! lup_decomposition.f90

! L U Decomposition of a matrix A into L U P
!     P A = L U
!

! 11/3/96 JSS mofified from Fortran 77

module lup_decomposition
  implicit none
contains
subroutine lup_decomp(A, L, U, IDX)
  real, dimension(:,:), intent(in) :: A
  real, dimension(:,:), intent(out) :: L
  real, dimension(:,:), intent(out) :: U
  integer, dimension(:), intent(out) :: IDX
  integer :: n
  real :: P
  real :: T
  integer :: kp, kt, i, j, k

  n = size(A,dim=1)  ! should be square
  L = A
  do i = 1,n
    IDX(i)=i
  end do
  do k=1,n-1
    P=0.0

    do i=k,n
      if( abs(L(i,k)) > P) then
        P=abs(L(i,k))
        kp=i
      end if
    end do

    if(P == 0.0) then
      print *, 'singular matrix'
      return
    end if

    kt=IDX(k)
    IDX(k)=IDX(kp)
    IDX(kp)=kt
    do i=1,n
      T=L(k,i)
      L(k,i)=L(kp,i)
      L(kp,i)=T
    end do

    do i=k+1,n
      L(i,k)=L(i,k)/L(k,k)
      do j=k+1,n
        L(i,j)=L(i,j)-L(i,k)*L(k,j)
      end do
    end do
  end do

  do i=1,n
   do j=1,n
     if(i > j)then
       U(i,j)=0.0
     else
       U(i,j)=L(i,j)
       L(i,j)=0.0
     end if
    end do
    L(i,i)=1.0
  end do
  return
end subroutine lup_decomp

! given A decomposed into L, U, P  and given Y, solve for X
!  A x = y

subroutine lup_solve(L, U, IDX, X, Y)
  real, dimension(:,:), intent(in)  :: L
  real, dimension(:,:), intent(in)  :: U
  real, dimension(:), intent(out)   :: X
  real, dimension(:), intent(in)    :: Y
  integer, dimension(:), intent(in) :: IDX
  integer :: n
  real, dimension(size(Y)) :: B
  integer :: i, j
  real :: sum

  n = size(L,dim=1)  ! square matrix, all dimensions start with 1
  do i=1,n
    sum=0.0
    do j=1,i-1
      sum=sum+L(i,j)*B(j)
    end do
    B(i) = Y(IDX(i)) - sum
  end do

  do i=n,1,-1
    sum=0.0
    do j=i+1,n
      sum=sum+U(i,j)*X(j)
    end do
    X(i) = (B(i)-sum)/U(i,i)
  end do

  return
end subroutine lup_solve

subroutine make_perm(P, IDX)
  real, dimension(:,:), intent(out) :: P
  integer, dimension(:), intent(in) :: IDX
  integer :: n
  integer :: i, j

  n = size(IDX,dim=1)
  do i=1,n
    do j=1,n
      P(i,j)=0.0
    end do
  end do
  do i=1,n
    P(i,IDX(i))=1.0
  end do
  return
end subroutine make_perm
end module lup_decomposition