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=8), 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
            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 parse_pos(i, pos_string, pos)
        !This subroutine parses the pos command allowing for command which include inf
        integer, intent(in) ::  i !The dimension of the position
        character(len=100), intent(in) :: pos_string !The position string
        real(kind=dp), intent(out) :: pos !The output parsed position value

        integer :: iospara
        if(trim(adjustl(pos_string)) == 'inf') then 
            pos=box_bd(2*i)
        else if(trim(adjustl(pos_string)) == '-inf') then
            pos=box_bd(2*i-1)
        else if ((index(pos_string,'-') > 0).and.(index(pos_string,'inf')>0)) then 
            !Now extract the number we are reducing from infinity
            read(pos_string(index(pos_string,'-')+1:), *, iostat=iospara) pos
            pos = box_bd(2*i) - pos
        else if ((index(pos_string,'+') > 0).and.(index(pos_string,'inf')>0)) then 
            !Now extract the number we are reducing from infinity
            read(pos_string(index(pos_string,'+')+1:), *, iostat=iospara) pos
            pos = box_bd(2*i-1) + pos
        else if ((index(pos_string,'*') > 0).and.(index(pos_string,'inf')>0)) then 
            !Now extract the number we are reducing from infinity
            read(pos_string(index(pos_string,'*')+1:), *, iostat=iospara) pos
            pos = (box_bd(2*i)-box_bd(2*i-1))*pos
        else
            read(pos_string, *, iostat=iospara) pos
        end if

        if (iospara > 0) then 
            print *, "Error reading position argument ", trim(adjustl(pos_string)), ". Please reformat and try again."
        end if
    end subroutine parse_pos

    subroutine heapsort(a)
     
       integer, intent(in out) :: a(0:)
       integer :: start, n, bottom
       real :: temp
     
       n = size(a)
       do start = (n - 2) / 2, 0, -1
         call siftdown(a, start, n);
       end do
     
       do bottom = n - 1, 1, -1
         temp = a(0)
         a(0) = a(bottom)
         a(bottom) = temp;
         call siftdown(a, 0, bottom)
       end do
     
    end subroutine heapsort
     
    subroutine siftdown(a, start, bottom)
     
      integer, intent(in out) :: a(0:)
      integer, intent(in) :: start, bottom
      integer :: child, root
      real :: temp
     
      root = start
      do while(root*2 + 1 < bottom)
        child = root * 2 + 1
     
        if (child + 1 < bottom) then
          if (a(child) < a(child+1)) child = child + 1
        end if
     
        if (a(root) < a(child)) then
          temp = a(child)
          a(child) = a (root)
          a(root) = temp
          root = child
        else
          return
        end if  
      end do      
     
    end subroutine siftdown
end module subroutines