module subroutines
    use parameters
    use functions
    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

end module subroutines