program main

  real A(4,4)
  real A2(4,4)
  real A3(4,4)
  real AA(4,4)
  real Y(4)
  real X(4)
  real B(4)
  real B2(4)
  real B3(4)
  real BB(4)
  real L(4,4)
  real U(4,4)
  real Z(4)
  real P(4,4)
  integer IDX(4)
  integer, parameter :: n=4
  integer i, j, k
  real sum

  data A /1.0, 3.0, 2.0, 5.0, 4.0, 4.0, 3.0, 3.0, 5.0, 1.0, 1.0, 2.0, 1.0, 4.0, 3.0, 2.0/

  data B /3.5, 2.4, -1.2, 6.1/

  data A2/1.0, 2.0, 3.0, 4.0, 2.0, 2.0, 3.0, 4.0, 3.0, 3.0, 3.0, 4.0, 4.0, 4.0, 4.0, 4.0/

  data B2/30.0, 31.0, 34.0, 40.0/

  data A3/4.0, 4.0, 4.0, 4.0, 4.0, 3.0, 3.0, 3.0, 4.0, 3.0, 2.0, 2.0, 4.0, 3.0, 2.0, 1.0/

  data B3/16.0, 13.0, 11.0, 10.0/

  print *, 'LUP Decomposition test, case 1'

  do i=1,n
    BB(i)=B(i)
    do j=1,n
      AA(i,j)=A(i,j)
    end do
  end do

  print *, 'A=  ', A
  call lup_decomp(n, AA, L, U, IDX)
  print *, 'AA= ', AA
  print *, 'U=  ', U
  print *, 'L=  ', L
  print *, 'IDX= ', IDX
  print *, ''

  print *, 'B=  ', B
  call lup_solve(n, L, U, IDX, BB, X, Y)
  print *, 'BB= ', BB
  print *, 'Y=  ', Y
  print *, 'X=  ', X

  do i=1,n
    sum=0.0
    do j=1,n
      sum=sum+A(i,j)*X(j)
    end do
    Z(i)=sum
  end do
  print *, 'Z= ', Z
  print *, 'Z should equal B, alias y'
  call make_perm(n, P, IDX)
  print *, 'P= ', P
  do i=1,n
    do j=1,n
      sum=0.0
      do k=1,n
        sum=sum+P(i,k)*A(k,j)-L(i,k)*U(k,j)
      end do
      AA(i,j)=sum
    end do
  end do
  print *, 'P*A-L*U=0 ', AA
  print *, ' '


  print *, 'LUP Decomposition test, case 2'

  do i=1,n
    BB(i)=B2(i)
    do j=1,n
      AA(i,j)=A2(i,j)
    end do
  end do

  print *, 'A2= ', A2
  call lup_decomp(n, AA, L, U, IDX)
  print *, 'AA= ', AA
  print *, 'U=  ', U
  print *, 'L=  ', L
  print *, 'IDX= ', IDX
  print *, ''

  print *, 'B2= ', B2
  call lup_solve(n, L, U, IDX, BB, X, Y)
  print *, 'BB= ', BB
  print *, 'Y=  ', Y
  print *, 'X=  ', X

  do i=1,n
    sum=0.0
    do j=1,n
      sum=sum+A2(i,j)*X(j)
    end do
    Z(i)=sum
  end do
  print *, 'Z= ', Z
  print *, 'Z should equal B2, alias y'
  call make_perm(n, P, IDX)
  print *, 'P= ', P
  do i=1,n
    do j=1,n
      sum=0.0
      do k=1,n
        sum=sum+P(i,k)*A2(k,j)-L(i,k)*U(k,j)
      end do
      AA(i,j)=sum
    end do
  end do
  print *, 'P*A-L*U=0 ', AA
  print *, ' '


  print *, 'LUP Decomposition test, case 3'

  do i=1,n
    BB(i)=B3(i)
    do j=1,n
      AA(i,j)=A3(i,j)
    end do
  end do

  print *, 'A3= ', A3
  call lup_decomp(n, AA, L, U, IDX)
  print *, 'AA= ', AA
  print *, 'U=  ', U
  print *, 'L=  ', L
  print *, 'IDX= ', IDX
  print *, ''

  print *, 'B3= ', B3
  call lup_solve(n, L, U, IDX, BB, X, Y)
  print *, 'BB= ', BB
  print *, 'Y=  ', Y
  print *, 'X=  ', X

  do i=1,n
    sum=0.0
    do j=1,n
      sum=sum+A3(i,j)*X(j)
    end do
    Z(i)=sum
  end do
  print *, 'Z= ', Z
  print *, 'Z should equal B3, alias y'
  call make_perm(n, P, IDX)
  print *, 'P= ', P
  do i=1,n
    do j=1,n
      sum=0.0
      do k=1,n
        sum=sum+P(i,k)*A3(k,j)-L(i,k)*U(k,j)
      end do
      AA(i,j)=sum
    end do
  end do
  print *, 'P*A-L*U=0 ', AA
  print *, ' '

  print *, 'finished LUP Decomposition'
end

subroutine lup_decomp(n, A, L, U, IDX)
  real A(n,n)
  real L(n,n)
  real U(n,n)
  integer IDX(n)
  real P
  integer kp, kt, i, j, k

  print *, 'in LUP Decomp'
  do i = 1,n
    IDX(i)=i
  end do
  do k=1,n-1
    P=0.0

    do i=k,n
      if( abs(A(i,k)) .gt. P) then
        P=abs(A(i,k))
        kp=i
      end if
    end do

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

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

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

  do i=1,n
   do j=1,n
     if(i .gt.j)then
       L(i,j)=A(i,j)
       U(i,j)=0.0
     else
       U(i,j)=A(i,j)
       L(i,j)=0.0
     end if
    end do
    L(i,i)=1.0
  end do
  return
end

subroutine lup_solve(n, L, U, IDX, B, X, Y)
  real L(n,n)
  real U(n,n)
  real B(n)
  real X(n)
  real Y(n)
  integer IDX(n)
  integer n, i, j
  real sum

  print *, 'in LUP Solve'

  do i=1,n
    sum=0.0
    do j=1,i-1
      sum=sum+L(i,j)*Y(j)
    end do
    Y(i) = B(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) = (Y(i)-sum)/U(i,i)
  end do

  return
end

subroutine make_perm(n, P, IDX)
  integer n
  real P(n,n)
  integer IDX(n)

  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