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 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


    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 build_cell_list(numinlist, r_list, rc_off, cell_num, num_in_cell, cell_list, which_cell)
        !This subroutine builds a cell list based on rc_off
        
        !----------------------------------------Input/output variables-------------------------------------------

        integer, intent(in) :: numinlist !The number of points within r_list

        real(kind=dp), dimension(3,numinlist), intent(in) :: r_list !List of points to be used for the construction of 
                                                                    !the cell list.

        real(kind=dp), intent(in) :: rc_off ! Cutoff radius which dictates the size of the cells

        integer, dimension(3), intent(inout) :: cell_num !Number of cells in each dimension.

        integer, allocatable, intent(inout) :: num_in_cell(:,:,:) !Number of points within each cell

        integer, allocatable, intent(inout) :: cell_list(:,:,:,:) !Index of points from r_list within each cell.

        integer, dimension(3,numinlist), intent(out) :: which_cell !The cell index for each point in r_list

        !----------------------------------------Begin Subroutine -------------------------------------------

        integer :: i, j, cell_lim, c(3)
        real(kind=dp) :: box_len(3)
        integer, allocatable :: resize_cell_list(:,:,:,:)

        !First calculate the number of cells that we need in each dimension
        do i = 1,3
            box_len(i) = box_bd(2*i) - box_bd(2*i-1)
            cell_num(i) = int(box_len(i)/(rc_off/2))+1
        end do 

        !Initialize/allocate variables 
        cell_lim = 10
        allocate(num_in_cell(cell_num(1),cell_num(2),cell_num(3)), cell_list(cell_lim, cell_num(1), cell_num(2), cell_num(3)))

        !Now place points within cell
        do i = 1, numinlist
            !c is the position of the cell that the point belongs to 
            do j = 1, 3
                c(j) = int((r_list(j,i)-box_bd(2*j-1))/(rc_off/2)) + 1
            end do

            !Place the index in the correct position, growing if necessary
            num_in_cell(c(1),c(2),c(3)) = num_in_cell(c(1),c(2),c(3)) + 1
            if (num_in_cell(c(1),c(2),c(3)) > cell_lim) then 
                allocate(resize_cell_list(cell_lim+10,cell_num(1),cell_num(2),cell_num(3)))
                resize_cell_list(1:cell_lim, :, :, :) = cell_list
                resize_cell_list(cell_lim+1:, :, :, :) = 0
                call move_alloc(resize_cell_list, cell_list)
            end if

            cell_list(num_in_cell(c(1),c(2),c(3)),c(1),c(2),c(3)) = i
            which_cell(:,i) = c
        end do 

        return
    end subroutine build_cell_list


    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