!=======================================================================
! ch08_matrix_inverse.f90
!
! Compute the inverse of a small matrix by Doolittle LU factorization.
!
! Origin:
!   Based on Inverse.f90 by Alexander Godunov, December 2009.
!   The original no-pivot LU factorization and triangular solves are
!   retained. Comments, zero-pivot reporting, and verification have
!   been added for the Chapter 8 companion website.
!
! Example:
!   A = [ 3  2  4; 2 -3  1; 1  1  2 ].
!   The program solves A*x = e_k for each column e_k of the identity
!   and places the solutions in the columns of A^{-1}.
!
! Explicit inversion is seldom needed to solve A*x=b. If only x is
! needed, solve the system directly. This example shows how an LU
! factorization can be reused for several right-hand sides.
!=======================================================================
program ch08_matrix_inverse
implicit none

integer, parameter :: n = 3
double precision :: a(n,n), a_work(n,n), ainverse(n,n)
double precision :: element, max_error
integer :: i, j, k, info

! Each implied-DO fills one row of A.
data (a(1,j),j=1,n) / 3.0d0,  2.0d0, 4.0d0 /
data (a(2,j),j=1,n) / 2.0d0, -3.0d0, 1.0d0 /
data (a(3,j),j=1,n) / 1.0d0,  1.0d0, 2.0d0 /

write (*,'(a)') 'Matrix inverse from Doolittle LU factorization'
write (*,'(/,a)') 'Matrix A:'
do i = 1,n
   write (*,'(3f12.6)') (a(i,j),j=1,n)
end do

! Keep A for the identity check: the factorization changes a_work.
a_work = a
call inverse_lu(a_work,ainverse,n,info)

if (info /= 0) then
   write (*,'(/,a,i0)') 'Zero pivot at factorization step ',info
   write (*,'(a)') 'This no-pivot LU method cannot invert the matrix.'
   stop 1
end if

write (*,'(/,a)') 'Inverse matrix A^{-1}:'
do i = 1,n
   write (*,'(3f12.6)') (ainverse(i,j),j=1,n)
end do

! Multiply A and A^{-1} by explicit loops. Report the largest
! difference from the identity matrix without using MATMUL.
max_error = 0.0d0
do i = 1,n
   do j = 1,n
      element = 0.0d0
      do k = 1,n
         element = element + a(i,k)*ainverse(k,j)
      end do
      if (i == j) element = element - 1.0d0
      max_error = max(max_error,abs(element))
   end do
end do
write (*,'(/,a,es12.4)') 'Maximum |A*A^{-1}-I| = ',max_error

contains

subroutine inverse_lu(a,c,n,info)
!=======================================================================
! Form C = A^{-1} by Doolittle LU and repeated triangular solves.
!
! Input:
!   a(n,n) - coefficient matrix, overwritten during elimination
!   n      - matrix dimension (n >= 1)
!
! Output:
!   c(n,n) - inverse matrix if info = 0
!   info   - 0 on success; factorization step with a zero pivot
!
! Method:
!   Factor A = L*U, where L has unit diagonal. For each column k
!   of the identity, solve L*d=e_k and then U*x=d. Store x in
!   column k of c. The same factors are reused for all n columns.
!
! No row pivoting is performed, as in the original Inverse.f90.
! A nonsingular matrix can have a zero pivot in this factorization;
! in that case a pivoted LU solver is needed. Even when the inverse
! has a small identity-check error, a poorly conditioned matrix
! may still give inaccurate entries in its computed inverse.
!=======================================================================
implicit none
integer, intent(in) :: n
double precision, intent(inout) :: a(n,n)
double precision, intent(out) :: c(n,n)
integer, intent(out) :: info

double precision :: lower(n,n), upper(n,n), rhs(n), d(n), x(n)
double precision :: coeff
integer :: i, j, k

info = 0
c = 0.0d0
lower = 0.0d0
upper = 0.0d0
if (n < 1) then
   info = 1
   return
end if

! Forward elimination records each multiplier below the diagonal
! of L and updates the trailing entries of the working matrix A.
do k = 1,n-1
   if (a(k,k) == 0.0d0) then
      info = k
      return
   end if
   do i = k+1,n
      coeff = a(i,k)/a(k,k)
      lower(i,k) = coeff
      do j = k+1,n
         a(i,j) = a(i,j) - coeff*a(k,j)
      end do
   end do
end do
if (a(n,n) == 0.0d0) then
   info = n
   return
end if

! Complete L with ones on its diagonal. U is the upper triangle
! of the working matrix after elimination.
do i = 1,n
   lower(i,i) = 1.0d0
end do
do j = 1,n
   do i = 1,j
      upper(i,j) = a(i,j)
   end do
end do

! Solve n systems with the same LU factors, one for each unit vector.
do k = 1,n
   rhs = 0.0d0
   rhs(k) = 1.0d0

   ! Forward substitution: L*d = e_k. The diagonal of L is one.
   do i = 1,n
      d(i) = rhs(i)
      do j = 1,i-1
         d(i) = d(i) - lower(i,j)*d(j)
      end do
   end do

   ! Back substitution: U*x = d.
   do i = n,1,-1
      x(i) = d(i)
      do j = i+1,n
         x(i) = x(i) - upper(i,j)*x(j)
      end do
      x(i) = x(i)/upper(i,i)
   end do

   ! The solution of A*x=e_k is column k of the inverse.
   do i = 1,n
      c(i,k) = x(i)
   end do
end do
end subroutine inverse_lu

end program ch08_matrix_inverse