module subroutines
    use parameters
    use functions
    use box
    implicit none

    integer :: allostat, deallostat

    public
    contains

    !This subroutine is just used to break the code and exit on an error
    subroutine read_error_check(para, loc)

        integer, intent(in) :: para
        character(len=100), intent(in) :: loc

        if (para > 0) then 
            print *, "Read error in ", trim(loc), " because of ", para
            stop "Exit with error"
        end if

    end subroutine

    subroutine matrix_inverse(a, n, a_inv)

        integer :: i, j, k, piv_loc
        integer, intent(in) :: n
        real(kind = dp) :: coeff, sum_l, sum_u
        real(kind = dp), dimension(n) :: b, x, y, b_piv
        real(kind = dp), dimension(n, n) :: l, u, p
        real(kind = dp), dimension(n, n), intent(in) :: a
        real(kind = dp), dimension(n, n), intent(out) :: a_inv
        real(kind = dp), allocatable :: v(:), u_temp(:), l_temp(:), p_temp(:)

        l(:, :) = identity_mat(n)
        u(:, :) = a(:, :)
        p(:, :) = identity_mat(n)
        !LU decomposition with partial pivoting
        do j = 1, n-1
            
            allocate(v(n-j+1), stat = allostat)
            if(allostat /=0 ) then
                print *, 'Fail to allocate v in matrix_inverse'
                stop
            end if   

              v(:) = u(j:n, j)
              if(maxval(abs(v)) < lim_zero) then
                    print *, 'Fail to inverse matrix', a
                    stop
              end if

            piv_loc = maxloc(abs(v), 1)
            deallocate(v, stat = deallostat)

            if(deallostat /=0 ) then
                print *, 'Fail to deallocate v in matrix_inverse'
                stop
            end if
        !partial pivoting
            if(piv_loc /= 1) then

                allocate( u_temp(n-j+1), p_temp(n), stat = allostat)
                if(allostat /=0 ) then
                    print *, 'Fail to allocate p_temp and/or u_temp in matrix_inverse'
                    stop
                end if

                u_temp(:) = u(j, j:n)
                u(j, j:n) = u(piv_loc+j-1, j:n)
                u(piv_loc+j-1, j:n) = u_temp(:)
                p_temp(:) = p(j, :)
                p(j, :) = p(piv_loc+j-1, :)
                p(piv_loc+j-1, :) = p_temp(:)

                deallocate( u_temp, p_temp, stat = deallostat)
                if(deallostat /=0 ) then
                    print *, 'Fail to deallocate p_temp and/or u_temp in matrix_inverse'
                    stop
                end if

                if(j > 1) then
                    
                    allocate( l_temp(j-1), stat = allostat)
                    if(allostat /= 0) then
                        print *, 'Fail to allocate l_temp in matrix_inverse'
                        stop
                    end if
                  
                    l_temp(:) = l(j, 1:j-1)
                    l(j, 1:j-1) = l(piv_loc+j-1, 1:j-1)
                    l(piv_loc+j-1, 1:j-1) = l_temp(:)
                    
                    deallocate( l_temp, stat = deallostat)
                    if(deallostat /=0 ) then
                        print *, 'Fail to deallocate l_temp in matrix_inverse'
                        stop
                    end if
                end if
            end if

            !LU decomposition
            do i = j+1, n
                coeff = u(i, j)/u(j, j)
                l(i, j) = coeff
                u(i, j:n) = u(i, j:n)-coeff*u(j, j:n)
            end do

        end do

        a_inv(:, :) = 0.0_dp
        do j = 1, n
            b(:) = 0.0_dp
            b(j) = 1.0_dp
            b_piv(:) = matmul(p, b)
            !Now we have LUx = b_piv
            !the first step is to solve y from Ly = b_piv
            !forward substitution
            do i = 1, n
                if(i == 1) then
                    y(i) = b_piv(i)/l(i, i)
                else
                    sum_l = 0
                    do k = 1, i-1
                        sum_l = sum_l+l(i, k)*y(k)
                    end do
                    y(i) = (b_piv(i)-sum_l)/l(i, i)
                end if
            end do
            !then we solve x from ux = y
            !backward subsitution
            do i = n, 1, -1
                if(i == n) then
                    x(i) = y(i)/u(i, i)
                else
                    sum_u = 0
                    do k = i+1, n
                        sum_u = sum_u+u(i, k)*x(k)
                    end do
                    x(i) = (y(i)-sum_u)/u(i, i)
                end if
            end do
            !put x into j column of a_inv
            a_inv(:, j) = x(:)
        end do
    return
    end subroutine matrix_inverse

    subroutine parse_ori_vec(ori_string, ori_vec)
        !This subroutine parses a string to vector in the format [ijk]
        character(len=20), intent(in) :: ori_string
        real(kind=dp), dimension(3), intent(out) :: ori_vec

        integer :: i, ori_pos, stat

        ori_pos=2
        do i = 1,3
            do while(ori_string(ori_pos:ori_pos)==' ')    
                ori_pos=ori_pos+1
            end do
            if (ori_string(ori_pos:ori_pos) == '-') then 
                ori_pos = ori_pos + 1
                read(ori_string(ori_pos:ori_pos), *, iostat=stat) ori_vec(i)
                if (stat>0) STOP "Error reading orientation value"
                ori_vec(i) = -ori_vec(i)
                ori_pos = ori_pos + 1
            else
                read(ori_string(ori_pos:ori_pos), *, iostat=stat) ori_vec(i)
                if(stat>0) STOP "Error reading orientation value"
                ori_pos=ori_pos + 1
            end if
        end do

        return
    end subroutine parse_ori_vec
    



    subroutine apply_periodic(r)
        !This function checks if an atom is outside the box and wraps it back in. This is generally used when some 
        !kind of displacement is applied but the simulation cell is desired to be maintained as the same size.

        real(kind=dp), dimension(3), intent(inout) :: r

        integer :: j 
        real(kind=dp) ::box_len
        do j = 1, 3
            if(box_bc(j:j) == 'p') then 
                box_len = box_bd(2*j) - box_bd(2*j-1)
                if (r(j) > box_bd(2*j)) then 
                    r(j) = r(j) - box_len
                else if (r(j) < box_bd(2*j-1)) then 
                    r(j) = r(j) + box_len
                end if
            end if
        end do
    end subroutine

    subroutine check_right_ortho(ori, isortho, isrighthanded)
        !This subroutine checks whether provided orientations in the form:
        ! | x1  x2  x3 |
        ! | y1  y2  y3 |
        ! | z1  z2  z3 |
        !are right handed
        
        real(kind=dp), dimension(3,3), intent(in) :: ori
        logical, intent(out) :: isortho, isrighthanded

        integer :: i, j 
        real(kind=dp) :: v(3), v_k(3)
        !Initialize variables
        isortho = .true.
        isrighthanded=.true.

        do i = 1, 3
            do j = i+1, 3

                if(abs(dot_product(ori(i,:), ori(j,:))) > lim_zero) then 
                    isortho = .false.
                end if

                !Check if they are righthanded
                if (j == i+1) then 
                    v(:) = cross_product(ori(i,:), ori(j,:))
                    v_k(:) = v(:) - ori(mod(j, 3)+1,:)
                else if ((i==1).and.(j==3)) then 
                    v(:) = cross_product(ori(j,:),ori(i,:))
                    v_k(:) = v(:) - ori(i+1, :)
                end if

                if(norm2(v_k) > 10.0_dp**(-8.0_dp)) then 
                    isrighthanded=.false.
                end if

            end do 

        end do

        return
    end subroutine check_right_ortho


end module subroutines