module linear
! Routines for linear algebra:
! - LU decomposition of a matrix
! - solution of a set of linear equations
! - inverse matrix

implicit none

contains

subroutine lu (A, order, n)
! Find the Lu decomposition of the matrix A(n,n).
! The original matrix is overwritten by the decomposition.
! The output parameter order(n) is a vector giving the
! permuted order of the matrix after pivoting.
! N.B.: the product of the decomposition is the permuted
! version of the original matrix

  integer, intent(in) :: n
  real, dimension(n,n), intent(inout) :: A
  integer, dimension(n), intent(out) :: order

  real, dimension(n) :: tmp
  real :: s, maxv
  integer i,j,k,l, m, maxind, t

  ! original order of the lines
  do i=1,n
     order(i) = i
  end do

  ! first line
  ! pivoting, find the largest element on the 1. column
  maxind = 1
  maxv = A(1,1)
  do l=2,n
     if (A(l, 1) > maxv) then
        maxv = A(l, 1) ; maxind = l
     end if
  end do
  ! exchange the line found with the first line
  tmp=A(maxind,:)
  A(maxind,:) = A(1,:)
  A(1,:)=tmp
  ! keep track of the order of the lines
  t = order(maxind)
  order(maxind) = 1
  order(1) = t

  do j=2,n 
     A(1,j)=A(1,j)/A(1,1)
  end do

  ! other lines
  do m=2,n
     ! first the column
     ! pivoting, find the largest element below A(m,m)
     maxind = m
     maxv = A(m,m)
     do l=m+1,n
       if (A(l, m) > maxv) then
          maxv = A(l, m) ; maxind = l
       end if
     end do
     ! exchange lines
     tmp=A(maxind,:)
     A(maxind,:) = A(m,:)
     A(m,:)=tmp
     t = order(maxind)
     order(maxind) = m
     order(m) = t

     ! handle the column below A(m,m)
     do i=m,n
        s=0.0
        do k=1,m-1 
           s=s+A(i,k)*A(k,m)
        end do
        A(i,m)=A(i,m)-s
     end do

     ! then the line to the right of A(m,m)
     do j=m+1,n
        s=0.0
        do k=1,m-1 
           s=s+A(m,k)*A(k,j)
        end do
        A(m,j)=(A(m,j)-s)/A(m,m)
     end do
   end do
end subroutine

subroutine solve (A, b, order, n, x)
! Solve the equation Ax=b.
! A(n, n) contains the LU decomposition of the coefficient matrix,
! b is the right-hand side of the equation.
! order is the vector returned by the subroutine lu, giving the
! permuted order of the lines of the matrix.
! The solution will be returned in the vector x(n), 
! the order of which will correspond to the original matrix

  integer, intent(in) :: n
  real, dimension(n,n), intent(in) :: A
  integer, dimension(n), intent(in) :: order
  real, dimension(n), intent(in) :: b
  real, dimension(n) :: x
  integer i,k
  real s    

  ! permute the right-hand side to correspond to the coefficien matrix
  do i=1,n
     x(order(i)) = b(i)
  end do

  x(1) = x(1)/A(1,1)

  do i=2,n
     s=0.0
     do k=1,i-1
        s=s+A(i,k)*x(k)
     end do
     x(i) = (x(i)-s) / A(i,i)
  end do  

  do i=n-1,1,-1
     s=x(i)
     do k=i+1,n
        s=s-A(i,k)*x(k)
     end do
     x(i)=s
  end do

end subroutine

function inverse (A, order, n)
! Find the inverse of the matrix A(n,n).
! Before calling the function A must be replaced by its LU decomposition.
! order is the vector returned by the subroutine lu, giving the
! permuted order of the lines of the matrix.

  integer, intent(in) :: n
  real, dimension(n,n), intent(in) :: A
  integer, dimension(n), intent(in) :: order
  real, dimension(n,n) :: inverse, C, S
  integer i

  C=0.0
  do i=1,n
     C(i,i)=1.0
  end do

  do i=1,n
     call solve(A, C(:,i), order, n, S(:,i))
  end do

  inverse = S

end function

function reorder (A, order, n)
! permute the lines of the matrix A(n,n) in the order 
! given in the vector order

  real, dimension(n,n) :: reorder
  integer, intent(in) :: n
  real, dimension(n,n), intent(in) :: A
  integer, dimension(n), intent(in) :: order
  real, dimension(n,n) :: C
  integer i

  do i=1,n
     C(order(i),:)=A(i,:)
  end do

  reorder = C
end function

subroutine dump(A, n, s)
! Write the matrix A(n,n)
! The string s is the title text

  integer, intent(in) :: n
  real, intent(in), dimension(n,n) :: A
  character(len=*) :: s
  integer i

  write(*,'(/A)') s
  do i=1,n
     write(*,'(10F6.2)') A(i,:)
  end do

end subroutine

end module