module mode_metric
    !This mode is used to calculate continuum metrics for the j

    use parameters
    use io
    use elements
    use neighbors

    implicit none
    
    integer :: nfiles
    character(len=100) :: metric_type
    real(kind=dp) :: rc_off, old_box_len(3)

    !Save reference positions
    integer :: np, npreal, nmet
    real(kind=dp), allocatable :: r_zero(:,:), r_curr(:,:), met(:,:)

    public 
    contains
    subroutine metric(arg_pos)
       !This is the main calling subroutine for the metric code 
        integer, intent(out) :: arg_pos
        character(len=100) :: infile, outfile

        integer :: i, ibasis, inod, np_temp, ppos
        real(kind=dp), dimension(6) :: temp_box_bd
        
        !These are the variables containing the cell list information
        integer, dimension(3) :: cell_num
        integer, allocatable :: num_in_cell(:,:,:), which_cell(:,:)
        integer, allocatable :: cell_list(:,:,:,:)

        !Parse the command arguments
        call parse_command(arg_pos)
    
        !Now read the first file 
        call read_in(1, (/ 0.0_dp, 0.0_dp, 0.0_dp /), temp_box_bd)
        np = atom_num + max_basisnum*max_ng_node*ele_num
        allocate(r_zero(3,atom_num+max_basisnum*max_ng_node*ele_num), &
                 r_curr(3,atom_num+max_basisnum*max_ng_node*ele_num)) 
        r_zero(:,:) = -huge(1.0_dp)

        !Set up the met variable for the user desired metric
        select case(trim(adjustl(metric_type)))
        case('def_grad')
            allocate(met(9, np))
        case('microrotation')
            allocate(met(4,np))
        end select

        !Now set the reference positions
        print *, "Combining positions"
        call convert_positions(r_zero, npreal)

        !Now calculate the neighbor list for the reference configuration
        print *, "Getting initial neighbor list"
        call calc_neighbor(rc_off, r_zero, np)

        !Save box lengths for applying periodic boundaries 
        do i=1,3
            old_box_len(i) = box_bd(2*i) - box_bd(2*i-1)
        end do
        !Reset element and box
        call reset_data
        call reset_box 

        !Now loop over new files
        do i = 2, nfiles
            
            call read_in(i, (/ 0.0_dp, 0.0_dp, 0.0_dp /), temp_box_bd)
            call convert_positions(r_curr, np_temp)
            if (npreal /= np_temp) then 
                print *, "Error in mode_metric where number of points in ", i, "th file is ", np_temp, " and number of points in" &
                // "reference file is", npreal
            end if
            print *, "Calculating metric for file number ", i
            call calc_metric
            !Now create the output file num and write out to xyz format
            ppos = scan(trim(infiles(i)),".", BACK= .true.)
            if ( ppos > 0 ) then 
                outfile = infiles(i)(1:ppos)//'xyz'
            else 
                outfile = infiles(i)//'.xyz'
            end if
            call write_metric_xyz(outfile)
            call reset_data
            call reset_box
        end do
    end subroutine metric

    subroutine parse_command(arg_pos)
        !This subroutine parses the arguments for mode metric
        integer, intent(out) :: arg_pos

        integer :: i, arglen
        character(len=100) :: textholder
        logical :: file_exists

        !First read the metric to be used
        call get_command_argument(2,metric_type,arglen)
        if (arglen == 0) stop "Incomplete mode metric command, check documentation"
        select case(trim(adjustl(metric_type))) 
        case("microrotation", "def_grad")
            continue
        case default
            print *, "Mode metric does not accept metric ", metric_type, ". Please select from: microrotation, def_grad"
            stop 3
        end select

        !Now read the cutoff radius
        call get_command_argument(3,textholder,arglen)
        if (arglen == 0) stop "Incomplete mode metric command, check documentation"
        read(textholder, *) rc_off

        !Now read the number of files to read and allocate the variables
        call get_command_argument(4, textholder, arglen)
        if (arglen == 0) stop "Incomplete mode metric command, check documentation"
        read(textholder, *) nfiles

        !Now read the files to be read 
        do i = 1, nfiles
            call get_command_argument(4+i, textholder, arglen)
            call get_in_file(textholder)
        end do 

        arg_pos = 5+nfiles
        return
    end subroutine parse_command

    subroutine calc_metric
        !This subroutine calculates the continuum metric that we require
        integer :: i, j, k, nei, ip, jp
        real(kind=dp) :: def_grad(3,3), omega(3,3), eta(3,3), rij(3), eta_inv(3,3), ftf(3,3), &
                         U(3,3), R(3,3), Rskew(3,3), oldrij(3), box_len(3)

        do i = 1, 3
            box_len(i) = box_bd(2*i) - box_bd(2*i-1)
        end do
        !Loop over all points
        do ip = 1, np
            eta(:,:) = 0.0_dp
            omega(:,:) = 0.0_dp
            def_grad(:,:) = 0.0_dp
            do jp = 1, nei_num(ip)
                !Calculate the neighbor vec in current configuration
                nei = nei_list(jp, ip)
                rij = r_curr(:,nei) - r_curr(:,ip)  +periodvec(:,jp,ip)*box_len 
                if (norm2(rij) > 5*rc_off) then 
                    do j = 1, 3
                        if (r_curr(j,nei) > r_curr(j,ip)) then 
                            rij(j) = rij(j) - box_len(j)
                        else if(r_curr(j,nei) < r_curr(j,ip)) then 
                            rij(j) = rij(j) + box_len(j)
                        end if
                    end do
                end if
                oldrij = r_zero(:,nei) - r_zero(:,ip) + periodvec(:,jp,ip)*old_box_len 

                !Calculate  eta and omega
                do i = 1,3
                    do j = 1,3
                        omega(i,j) = omega(i,j) + rij(i) * oldrij(j)
                        eta(i,j) = eta(i,j) + oldrij(i) * oldrij(j)
                    end do
                end do 
            end do
            
            eta_inv=matinv3(eta)
            def_grad=matmul(omega,eta_inv)

            select case(trim(adjustl(metric_type)))
                case('def_grad')
                    k = 1
                    do i = 1,3
                        do j = 1, 3
                            met(k, ip) = def_grad(i,j)   
                            k = k + 1
                        end do
                    end do
                case('microrotation')
                    met(:,ip) = 0.0_dp
                    if(.not.all(def_grad == 0)) then 
                        !Now calculate microrotation
                        ftf = matmul(transpose3(def_grad), def_grad)
                        U = sqrt3(ftf)
                        if(.not.all(abs(U) < lim_zero)) then 
                            R = matmul(def_grad, matinv3(U))
                            Rskew = 0.5_dp * ( R - transpose3(R))
                            do k =1,3
                                do j = 1,3
                                    do i = 1,3
                                        met(k,ip) = met(k,ip) -0.5*permutation(i,j,k)*Rskew(i,j)
                                    end do
                                end do
                            end do
                            met(4,ip) = norm2(met(1:3,ip))
                        end if
                    end if
            end select
        end do
        return 
    end subroutine

    subroutine convert_positions(rout, npoints)
        !This subroutine just converts current atom and element arrays to a single point based form
        real(kind=dp), dimension(3,atom_num+max_ng_node*max_basisnum*ele_num), intent(inout)  :: rout
        integer, intent(out) :: npoints

        integer :: i, inod, ibasis

        npoints=0
        rout = -huge(1.0_dp)
        if(atom_num > 0) then 
            do i = 1, atom_num
                call apply_periodic(r_atom(:,i))
                rout(:,tag_atom(i))  = r_atom(:,i)
                npoints = npoints + 1
            end do
        end if

        if (ele_num > 0) then 
            do i = 1, ele_num
                do inod = 1, ng_node(lat_ele(i))
                    do ibasis = 1, basisnum(lat_ele(i))
                        call apply_periodic(r_node(:,ibasis,inod,i))
                        rout(:, atom_num+(tag_ele(i)-1)*max_ng_node*max_basisnum + (inod-1)*max_basisnum + ibasis) &
                            = r_node(:,ibasis,inod,i)
                                

                        npoints = npoints + 1
                    end do
                end do
            end do
        end if

    end subroutine convert_positions

    subroutine write_metric_xyz(outfile)
        character(len=100), intent(in) :: outfile

        integer :: i, inod, ibasis
        real(kind = dp) :: r(3), eng
        open (unit=11, file=trim(adjustl(outfile)), action='write', position='rewind', status = 'replace')
        !Write the header
        write(11,*) npreal
        
        select case(metric_type)
        case('def_grad')
            write(11,*) "id type element x y z F11 F12 F13 F21 F22 F23 F31 F32 F33"
        case('microrotation')
            write(11,*) "id type element x y z micro1 micro2 micro3 norm2(micro)"
        end select

        if(atom_num > 0) then 
            do i = 1, atom_num
                write(11,*) tag_atom(i), type_atom(i), 0, r_atom(:,i), met(:,tag_atom(i))
            end do
        end if

        if (ele_num > 0) then 
            do i = 1, ele_num
                do inod = 1, ng_node(lat_ele(i))
                    do ibasis = 1, basisnum(lat_ele(i))
                        write(11,*) atom_num+(tag_ele(i)-1)*max_ng_node*max_basisnum + (inod-1)*max_basisnum + ibasis, &
                                    basis_type(ibasis,lat_ele(i)), 1, r_node(:,ibasis,inod,i), &
                                    met(:, atom_num+(tag_ele(i)-1)*max_ng_node*max_basisnum + (inod-1)*max_basisnum + ibasis) 
                    end do
                end do
            end do
        end if
    end subroutine write_metric_xyz

end module mode_metric
