!=======================================================================
! ch08_shifted_inverse_iteration.f90
!
! Find an eigenpair near a chosen shift by inverse iteration.
!
! Origin:
!   New companion implementation of the shifted inverse power method
!   described in Chapter 8 by Alexander Godunov. The vector step is
!   (A-shift*I)*z = y, followed by y = z/||z||. A pivoted LU
!   factorization is computed once and reused for every solve.
!
! Example:
!   A = [ 1  2  3; 2  2 -2; 3 -2  4 ], shift = 3.4.
!   The eigenvalues are approximately -2.54138127, 3.54138127,
!   and 6.00000000; the eigenvalue closest to 3.4 is 3.54138127.
!
! A shift equal to an eigenvalue makes A-shift*I singular. A shift
! very close to an eigenvalue makes it ill-conditioned. The method
! finds one eigenpair, provided the initial vector has a component
! along the desired eigenvector and the selected eigenvalue is isolated.
!=======================================================================
program ch08_shifted_inverse_iteration
implicit none

integer, parameter :: n = 3, max_iter = 100
double precision, parameter :: shift = 3.4d0, eps = 1.0d-10
double precision :: a(n,n), y(n), lambda, residual
integer :: i, j, iterations, pivot_step, info

! Each implied-DO fills one row of the symmetric example matrix.
data (a(1,j),j=1,n) / 1.0d0,  2.0d0,  3.0d0 /
data (a(2,j),j=1,n) / 2.0d0,  2.0d0, -2.0d0 /
data (a(3,j),j=1,n) / 3.0d0, -2.0d0,  4.0d0 /
data (y(i),i=1,n)   / 1.0d0,  1.0d0,  1.0d0 /

write (*,'(a)') 'Shifted inverse iteration for one eigenpair'
write (*,'(/,a)') 'Matrix A:'
do i = 1,n
   write (*,'(3f12.6)') (a(i,j),j=1,n)
end do
write (*,'(/,a,3f12.6)') 'Initial vector:',y
write (*,'(a,f12.6)') 'Shift = ',shift
write (*,'(a,es12.4)') 'Eigenpair residual tolerance = ',eps

call shifted_inverse(a,y,n,shift,eps,max_iter,lambda,iterations, &
                     residual,pivot_step,info)
if (info == 1) then
   write (*,'(/,a)') 'Invalid input: check symmetry, initial vector, and controls.'
   stop 1
end if
if (info == 2) then
   write (*,'(/,a,i0)') 'A-shift*I is singular or nearly singular at pivot step ',pivot_step
   write (*,'(a)') 'Choose a different shift.'
   stop 1
end if
if (info == 3) then
   write (*,'(/,a)') 'The inverse iteration vector is zero; cannot normalize.'
   stop 1
end if

write (*,'(/,a,f12.6)') 'Eigenvalue near shift = ',lambda
write (*,'(a)') 'Unit eigenvector:'
do i = 1,n
   write (*,'(a,i0,a,f12.6)') 'y(',i,') = ',y(i)
end do
write (*,'(a,i0)') 'Iterations = ',iterations
write (*,'(a,es12.4)') 'Eigenpair residual ||A*y-lambda*y|| = ',residual
if (info == 4) then
   write (*,'(a)') 'Iteration limit reached before the tolerance was met.'
   stop 1
end if

contains

subroutine shifted_inverse(a,y,n,shift,eps,max_iter,lambda,iterations, &
                           residual,pivot_step,info)
!=======================================================================
! Approximate the eigenvalue of a real symmetric A nearest shift.
!
! Input:
!   a(n,n)     - symmetric matrix, left unchanged
!   y(n)       - nonzero initial vector, overwritten by a unit vector
!   shift      - fixed estimate of the desired eigenvalue
!   eps        - positive tolerance for ||A*y-lambda*y||_2
!   max_iter   - maximum number of inverse iterations
!
! Output:
!   lambda     - signed Rayleigh quotient y^T*A*y / (y^T*y)
!   iterations - completed inverse iterations
!   residual   - Euclidean norm of the eigenpair residual
!   pivot_step - small-pivot step, if info = 2
!   info       - 0 converged; 1 invalid input; 2 singular shifted
!                matrix; 3 zero inverse vector; 4 iteration limit
!
! Factor A-shift*I once with row pivoting. At each step, solve the
! shifted system rather than forming an inverse matrix. Normalize the
! solution, compute a Rayleigh quotient from the ORIGINAL A, and test
! the full eigenpair residual. A stable eigenvalue estimate alone is
! not a reliable stopping test.
!=======================================================================
implicit none
integer, intent(in) :: n, max_iter
double precision, intent(in) :: a(n,n), shift, eps
double precision, intent(inout) :: y(n)
double precision, intent(out) :: lambda, residual
integer, intent(out) :: iterations, pivot_step, info

double precision :: lu(n,n), z(n), ay(n), norm_y, norm_z
integer :: ipiv(n), i, j, k

lambda = 0.0d0
residual = huge(1.0d0)
iterations = 0
pivot_step = 0
info = 1
if (n < 1 .or. eps <= 0.0d0 .or. max_iter < 1) return
do i = 1,n
   do j = i+1,n
      if (a(i,j) /= a(j,i)) return
   end do
end do

norm_y = sqrt(sum(y*y))
if (norm_y == 0.0d0) return
y = y/norm_y

! Subtract the shift from each diagonal entry, retaining A for the
! Rayleigh quotient and the independent residual calculation.
lu = a
do i = 1,n
   lu(i,i) = lu(i,i)-shift
end do
call lu_factor(lu,n,ipiv,pivot_step)
if (pivot_step /= 0) then
   info = 2
   return
end if

do k = 1,max_iter
   ! The same factors solve a new right-hand side at every step.
   call lu_solve(lu,ipiv,y,z,n)
   norm_z = sqrt(sum(z*z))
   if (norm_z == 0.0d0) then
      info = 3
      return
   end if
   y = z/norm_z

   call matrix_vector(a,y,ay,n)
   lambda = sum(y*ay)
   residual = sqrt(sum((ay-lambda*y)**2))
   iterations = k
   if (residual <= eps) then
      info = 0
      return
   end if
end do
info = 4
end subroutine shifted_inverse

subroutine lu_factor(lu,n,ipiv,pivot_step)
!=======================================================================
! Factor the shifted matrix in place as P*B = L*U.
!
! The entries below the diagonal store the multipliers of unit-lower
! triangular L; the diagonal and upper part store U. At column k,
! choose the remaining row with largest |lu(i,k)|. Store that row in
! ipiv(k) so the same swaps can be applied to every right-hand side.
! Exchange entire rows, including multipliers already computed.
!
! pivot_step = 0 on success; otherwise it gives a zero or nearly zero
! pivot. The test is relative to the scale of the original shifted
! matrix, so a shift too close to an eigenvalue is reported explicitly.
!=======================================================================
implicit none
integer, intent(in) :: n
double precision, intent(inout) :: lu(n,n)
integer, intent(out) :: ipiv(n), pivot_step

double precision :: best, store, pivot_tol
integer :: i, j, k, pivot_row

pivot_step = 0
pivot_tol = 10.0d0*dble(n)*epsilon(1.0d0)*maxval(abs(lu))
do k = 1,n
   pivot_row = k
   best = abs(lu(k,k))
   do i = k+1,n
      if (abs(lu(i,k)) > best) then
         best = abs(lu(i,k))
         pivot_row = i
      end if
   end do
   ipiv(k) = pivot_row
   if (best <= pivot_tol) then
      pivot_step = k
      return
   end if

   if (pivot_row /= k) then
      do j = 1,n
         store = lu(k,j)
         lu(k,j) = lu(pivot_row,j)
         lu(pivot_row,j) = store
      end do
   end if

   do i = k+1,n
      lu(i,k) = lu(i,k)/lu(k,k)
      do j = k+1,n
         lu(i,j) = lu(i,j)-lu(i,k)*lu(k,j)
      end do
   end do
end do
end subroutine lu_factor

subroutine lu_solve(lu,ipiv,b,z,n)
!-----------------------------------------------------------------------
! Solve B*z=b with the stored P*B=L*U factorization. Apply the row
! swaps to b, then solve L*w=P*b and U*z=w by substitution.
!-----------------------------------------------------------------------
implicit none
integer, intent(in) :: n, ipiv(n)
double precision, intent(in) :: lu(n,n), b(n)
double precision, intent(out) :: z(n)
double precision :: store
integer :: i, j

z = b
do i = 1,n
   if (ipiv(i) /= i) then
      store = z(i)
      z(i) = z(ipiv(i))
      z(ipiv(i)) = store
   end if
end do
do i = 1,n
   do j = 1,i-1
      z(i) = z(i)-lu(i,j)*z(j)
   end do
end do
do i = n,1,-1
   do j = i+1,n
      z(i) = z(i)-lu(i,j)*z(j)
   end do
   z(i) = z(i)/lu(i,i)
end do
end subroutine lu_solve

subroutine matrix_vector(a,v,w,n)
!-----------------------------------------------------------------------
! Form A*v explicitly so the eigenvalue and residual use the original A.
!-----------------------------------------------------------------------
implicit none
integer, intent(in) :: n
double precision, intent(in) :: a(n,n), v(n)
double precision, intent(out) :: w(n)
integer :: i, j

do i = 1,n
   w(i) = 0.0d0
   do j = 1,n
      w(i) = w(i)+a(i,j)*v(j)
   end do
end do
end subroutine matrix_vector

end program ch08_shifted_inverse_iteration