module subroutines
    use parameters
    use functions
    use box
    use str

    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(*), intent(in) :: ori_string
        real(kind=dp), dimension(3), intent(out) :: ori_vec
        real(kind=dp) :: prefactor
        integer :: start, fin

        integer :: i, ori_pos, stat

        do i = 1, len(ori_string)
            if(ori_string(i:i) == "[") then 
                start=i+1
            else if(ori_string(i:i) == "]") then 
                fin = i-1
            end if
        end do
        
        ori_pos=start

        if (tok_count(ori_string(start:fin)) == 3) then 
            read(ori_string(start:fin),*) ori_vec(:)
        else
            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
        end if
        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

    subroutine init_random_seed()
        implicit none
        integer, allocatable :: seed(:)
        integer :: i, n, un, istat, dt(8), pid, t(2), s
        integer(8) :: count, tms
      
        call random_seed(size = n)
        allocate(seed(n))
        ! First try if the OS provides a random number generator
        open(newunit=un, file="/dev/urandom", access="stream", &
             form="unformatted", action="read", status="old", iostat=istat)
        if (istat == 0) then
           read(un) seed
           close(un)
        else
           ! Fallback to XOR:ing the current time and pid. The PID is
           ! useful in case one launches multiple instances of the same
           ! program in parallel.
           call system_clock(count)
           if (count /= 0) then
              t = transfer(count, t)
           else
              call date_and_time(values=dt)
              tms = (dt(1) - 1970) * 365_8 * 24 * 60 * 60 * 1000 &
                   + dt(2) * 31_8 * 24 * 60 * 60 * 1000 &
                   + dt(3) * 24 * 60 * 60 * 60 * 1000 &
                   + dt(5) * 60 * 60 * 1000 &
                   + dt(6) * 60 * 1000 + dt(7) * 1000 &
                   + dt(8)
              t = transfer(tms, t)
           end if
           s = ieor(t(1), t(2))
           pid = getpid() + 1099279 ! Add a prime
           s = ieor(s, pid)
           if (n >= 3) then
              seed(1) = t(1) + 36269
              seed(2) = t(2) + 72551
              seed(3) = pid
              if (n > 3) then
                 seed(4:) = s + 37 * (/ (i, i = 0, n - 4) /)
              end if
           else
              seed = s + 37 * (/ (i, i = 0, n - 1 ) /)
           end if
        end if
        call random_seed(put=seed)
      end subroutine init_random_seed

end module subroutines
