module functions

    use parameters

    implicit none

    public
    contains


    ! Functions below this comment are taken from the functions module of atomsk
    !********************************************************
    !  STRUPCASE
    !  This function reads a string of any length
    !  and capitalizes all letters.
    !********************************************************
    FUNCTION StrUpCase (input_string) RESULT (UC_string)
    !
    IMPLICIT NONE
    CHARACTER(*),PARAMETER:: lower_case = 'abcdefghijklmnopqrstuvwxyz'
    CHARACTER(*),PARAMETER:: upper_case = 'ABCDEFGHIJKLMNOPQRSTUVWXYZ'
    CHARACTER(*),INTENT(IN):: input_string
    CHARACTER(LEN(Input_String)):: UC_string !Upper-Case string that is produced
    INTEGER:: i, n
    !
    IF(LEN(input_string)==0) RETURN
    UC_string = input_string
    ! Loop over string elements
    DO i=1,LEN(UC_string)
      !Find location of letter in lower case constant string
      n = INDEX( lower_case, UC_string(i:i) )
      !If current substring is a lower case letter, make it upper case
      IF(n>0) THEN
        UC_string(i:i) = upper_case(n:n)
      ENDIF
    END DO
    !
    END FUNCTION StrUpCase

    !********************************************************
    !  STRDNCASE
    !  This function reads a string of any length
    !  and transforms all letters to lower case.
    !********************************************************
    FUNCTION StrDnCase (input_string) RESULT (lc_string)
    !
    IMPLICIT NONE
    CHARACTER(*),PARAMETER:: lower_case = 'abcdefghijklmnopqrstuvwxyz'
    CHARACTER(*),PARAMETER:: upper_case = 'ABCDEFGHIJKLMNOPQRSTUVWXYZ'
    CHARACTER(*),INTENT(IN):: input_string
    CHARACTER(LEN(Input_String)):: lc_string !Lower-Case string that is produced
    INTEGER:: i, n
    !
    IF(LEN(input_string)==0) RETURN
    lc_string = input_string
    ! Loop over string elements
    DO i=1,LEN(lc_string)
      !Find location of letter in upper case constant string
      n = INDEX( upper_case, lc_string(i:i) )
      !If current substring is an upper case letter, make it lower case
      IF(n>0) THEN
        lc_string(i:i) = lower_case(n:n)
      ENDIF
    END DO
!
END FUNCTION StrDnCase

    pure function matrix_normal(a, n)

        integer :: i
        integer, intent(in) :: n
        real(kind = dp), dimension(n) :: v
        real(kind = dp), dimension(n, n),intent(in) :: a
        real(kind = dp), dimension(n,n) :: matrix_normal

        matrix_normal(:, :) = a(:, :)

        do i = 1, n

            v(:) = a(i,:)

            matrix_normal(i, :) = v(:) / norm2(v)

        end do

        return
    end function matrix_normal

    pure function cross_product(a, b)
        !Function which finds the cross product of two vectors

        real(kind = dp), dimension(3), intent(in) :: a, b
        real(kind = dp), dimension(3) :: cross_product

        cross_product(1) = a(2) * b(3) - a(3) * b(2)
        cross_product(2) = a(3) * b(1) - a(1) * b(3)
        cross_product(3) = a(1) * b(2) - a(2) * b(1)

        return
    end function cross_product

    pure function identity_mat(n)
        !Returns the nxn identity matrix

        integer :: i
        integer, intent(in) :: n
        real(kind = dp), dimension(n, n) :: identity_mat

        identity_mat(:, :) = 0.0_dp
        do i = 1, n
          identity_mat(i, i) = 1.0_dp
        end do

     return
     end function identity_mat

     pure function triple_product(a, b, c)
        !triple product between three 3*1 vectors

        real(kind = dp) :: triple_product
        real(kind = dp), dimension(3), intent(in) :: a, b, c
        triple_product = dot_product(a, cross_product(b, c))

     return
     end function triple_product

    function in_bd_lat(v, box_faces, box_norms)
        !This function returns whether the point is within the transformed box boundaries. The transformed 
        !box being the transformed simulation cell in the lattice basis
        
        !Input/output variables
        real(kind=dp), dimension(3), intent(in) :: v !integer lattice position
        real(kind=dp), dimension(3,6), intent(in) :: box_faces !Centroid of all the box faces
        real(kind=dp), dimension(3,6), intent(in) :: box_norms !Box face normals
        logical :: in_bd_lat

        !Other variables
        integer :: i
        real(kind=dp) :: pt_to_face(3)

        in_bd_lat = .true.

        !Check if point is in box bounds, this works by comparing the dot product of the face normal and the 
        !vector from the point to the face. If the dot product is greater than 0 then the point is behind the face
        !if it is equal to zero then the point is on the face, if is less than 0 the the point is in front of the face.
        do i = 1, 6
            pt_to_face(:) = box_faces(:, i) - v 
            if(dot_product(pt_to_face, box_norms(:,i)) <= 0) then 
                in_bd_lat = .false.
                exit
            end if
        end do

        return
    end function in_bd_lat

    function in_block_bd(v, box_bd)
        !This function determines whether a point is within a block in 3d
        !Input/output
        real(kind=dp), dimension(3), intent(in) :: v
        real(kind=dp), dimension(6), intent(in) :: box_bd
        logical :: in_block_bd

        !Other variables
        integer :: i

        in_block_bd = .true.

        do i =1 ,3
            !Check upper bound
            if(v(i) > (box_bd(2*i))) then 
                in_block_bd =.false.
                exit
            !Check lower bound
            else if (v(i) < (box_bd(2*i-1))+1d-6) then 
                in_block_bd = .false.
                exit
            end if
        end do
    end function in_block_bd

    function in_wedge_bd(r,vertex)
        !This code determines whether the 2dimensional projection of a point lies within the 2 dimensional projection of a wedge.
        real(kind=dp), intent(in) :: r(3) !This is the point position
        real(kind=dp), intent(in) :: vertex(3,3) !These are the relevant vertex positions for the wedge
        real(kind=dp) :: v1(3), v2(3), v3(3), c1(3), c2(3) !Vertex vector to point and cross products
        integer :: i
        logical :: in_wedge_bd

        in_wedge_bd = .true.
        do i = 1, 3
            v1 = vertex(:,mod(i,3)+1) - vertex(:,i)
            v2 = r - vertex(:,i)
            v3 = vertex(:,mod(i+1,3)+1) - vertex(:,i)
            c1 = cross_product(v1,v2)  
            c2 = cross_product(v1,v3)  
            if(dot_product(c1,c2) < 0) then 
               in_wedge_bd=.false.
               exit
            end if
        end do
    end function in_wedge_bd

    function lcm(a,b)
        !This function returns the smallest least common multiple of two numbers 

        real(kind=dp), intent(in) :: a, b
        real(kind=dp) :: lcm

        integer :: aint, bint, gcd, remainder, placeholder

        !Cast the vector positions to ints. There will be some error associated with this calculation
        aint = a*10**2
        bint = b*10**2

        !Calculate greated common divisor
        gcd = aint
        placeholder = bint
        do while(placeholder /= 0)
            remainder = modulo(gcd, placeholder)
            gcd = placeholder
            placeholder=remainder
        end do
        lcm = real((aint*bint),dp)/(real(gcd,dp))* 10.0_dp**(-2.0_dp)
    end function lcm

    function is_neighbor(rl, rk, r_in, r_out)
        !This function checks to see if two atoms are within a shell with an inner radius r_in and outer radius 
        !r_out
        real(kind=dp), intent(in) :: r_in, r_out
        real(kind=dp), dimension(3), intent(in) :: rl, rk
        logical :: is_neighbor

        !Internal variable
        real(kind=dp) :: rlk

        rlk = norm2(rk - rl)
        is_neighbor=.true.
        if((rlk>r_out).or.(rlk < r_in)) is_neighbor = .false.

        return
    end function is_neighbor
    
    function is_equal(A, B)
        !This function checks if too numbers are equal within a tolerance
        real(kind=dp), intent(in) :: A, B
        logical :: is_equal

        if((A>(B - 10.0_dp**(-10))).and.(A < (B+10.0_dp**(-10)))) then 
            is_equal = .true.
        else
            is_equal = .false.
        end if
    return
    end function is_equal

    function mass_is_equal(A, B)
        !This function checks if too numbers are equal within a tolerance
        real(kind=dp), intent(in) :: A, B
        logical :: mass_is_equal

        if((A>(B - 10.0_dp**(-2))).and.(A < (B+10.0_dp**(-2)))) then 
            mass_is_equal = .true.
        else
            mass_is_equal = .false.
        end if
    return
    end function mass_is_equal

    pure function unitvec(n,vec)
        integer, intent(in) :: n
        real(kind=dp), intent(in) :: vec(n)
        real(kind=dp) :: unitvec(n)

        unitvec = vec/norm2(vec)
        return
    end function unitvec

    pure function norm_dis(rl,rk)
        !This just returns the magnitude of the vector between two points
        real(kind=dp), dimension(3), intent(in) :: rl, rk
        real(kind=dp) :: norm_dis(4)

        norm_dis(1:3) = (rk - rl)
        norm_dis(4) = norm2(rk-rl)
    end function

    pure function matinv3(A) result(B)
        !! Performs a direct calculation of the inverse of a 3×3 matrix.
        real(kind=dp), intent(in) :: A(3,3)   !! Matrix
        real(kind=dp)             :: B(3,3)   !! Inverse matrix
        real(kind=dp)             :: detinv

        if(abs(A(1,1)*A(2,2)*A(3,3) - A(1,1)*A(2,3)*A(3,2)&
                  - A(1,2)*A(2,1)*A(3,3) + A(1,2)*A(2,3)*A(3,1)&
                  + A(1,3)*A(2,1)*A(3,2) - A(1,3)*A(2,2)*A(3,1)) < lim_zero) then 
            B(:,:) = 0
            return
        else
            ! Calculate the inverse determinant of the matrix
            
            detinv = 1/(A(1,1)*A(2,2)*A(3,3) - A(1,1)*A(2,3)*A(3,2)&
                      - A(1,2)*A(2,1)*A(3,3) + A(1,2)*A(2,3)*A(3,1)&
                      + A(1,3)*A(2,1)*A(3,2) - A(1,3)*A(2,2)*A(3,1))

            ! Calculate the inverse of the matrix
            B(1,1) = +detinv * (A(2,2)*A(3,3) - A(2,3)*A(3,2))
            B(2,1) = -detinv * (A(2,1)*A(3,3) - A(2,3)*A(3,1))
            B(3,1) = +detinv * (A(2,1)*A(3,2) - A(2,2)*A(3,1))
            B(1,2) = -detinv * (A(1,2)*A(3,3) - A(1,3)*A(3,2))
            B(2,2) = +detinv * (A(1,1)*A(3,3) - A(1,3)*A(3,1))
            B(3,2) = -detinv * (A(1,1)*A(3,2) - A(1,2)*A(3,1))
            B(1,3) = +detinv * (A(1,2)*A(2,3) - A(1,3)*A(2,2))
            B(2,3) = -detinv * (A(1,1)*A(2,3) - A(1,3)*A(2,1))
            B(3,3) = +detinv * (A(1,1)*A(2,2) - A(1,2)*A(2,1))
        end if
    end function

     pure function transpose3(A) result(B)
         !!Transposes matrix A
         real(kind=dp), intent(in) :: A(3,3)
         real(kind=dp)             :: B(3,3)  

         integer :: i, j
         forall(i =1:3,j=1:3) B(i,j) = A(j,i)

    end function transpose3

    pure function sqrt3(A) result(B)
        !This calculates the square of matrix A fulfilling the equation B*B = A according to 
        !the algorithm by Franca1989

        real(kind=dp), intent(in) :: A(3,3)
        real(kind=dp)             :: B(3,3)

        real(kind=dp) :: Ione, Itwo, Ithree, l, k, phi, Asq(3,3), lambda, Bone, Btwo, Bthree, p
    
        !Step 1 is calculating the invariants of C
        Ione = A(1,1) + A(2,2)  + A(3,3)
        Asq = matmul(A,A)
        Itwo = 0.5_dp *(Ione*Ione - (Asq(1,1) + Asq(2,2) + Asq(3,3)))
        Ithree = (A(1,1)*A(2,2)*A(3,3) - A(1,1)*A(2,3)*A(3,2)& 
                - A(1,2)*A(2,1)*A(3,3) + A(1,2)*A(2,3)*A(3,1)& 
                + A(1,3)*A(2,1)*A(3,2) - A(1,3)*A(2,2)*A(3,1)) 

        if (Ithree < 0) then 
            B(:,:)=0.0_dp
            return
        end if
        !Check for an isotropic matrix
        k = Ione*Ione - 3*Itwo
        if (k < lim_zero) then 
            lambda = sqrt(Ione/3.0_dp)
            B = lambda*identity_mat(3)
        else
            l = Ione*(Ione*Ione - 9.0_dp/2.0_dp * Itwo) + 27.0_dp/2.0_dp * Ithree
            p = l/(k**(1.5_dp))

            if (p > 1.0 ) then 
                B(:,:) = 0.0_dp
                return
            end if

            if ((p< -1).or.(p>1)) then 
                B(:,:) = 0.0_dp
                return
            end if
            phi = acos(p)
            lambda = sqrt(1.0_dp/3.0_dp * (Ione + 2*sqrt(k)*cos(phi/3)))
            
            !Now calculate invariantes of B
            Bthree = sqrt(Ithree)
            if((-lambda*lambda + Ione + 2*Ithree/lambda) < 0) then 
                B(:,:) = 0.0_dp
                return
            end if
            Bone = lambda + sqrt(- lambda*lambda + Ione + 2*Ithree/lambda)
            Btwo = (Bone*Bone - Ione)/2.0_dp
            
            !Now calculate B
            if(abs(Bone*Btwo -Bthree) < lim_zero) then 
                B(:,:) = 0.0_dp
                return
            end if
            B = 1/(Bone*Btwo - Bthree) *(Bone*Bthree*identity_mat(3) + (Bone*Bone - Btwo)*A - matmul(A,A))
        end if
    end function sqrt3

    pure function permutation(i,j,k) result(e)
        !Calculates the permutation tensor
        integer, intent(in) :: i,j,k
        integer             :: e
        
        if ( ((i==1).and.(j==2).and.(k==3)).or. &
             ((i==2).and.(j==3).and.(k==1)).or. &
             ((i==3).and.(j==1).and.(k==2))) then
            e=1
        else if( ((i==2).and.(j==1).and.(k==3)).or. &
                 ((i==1).and.(j==3).and.(k==2)).or. &
                 ((i==3).and.(j==2).and.(k==1))) then
            e=-1
        else
            e=0
        end if
    end function permutation

    pure function evtogp(virial)
        real(kind=dp), dimension(6), intent(in) :: virial
        real(kind=dp), dimension(6) :: evtogp

        evtogp = virial * 1e21_dp * 1.602176565e-19_dp

    end function

end module functions