%=======================================================================
% ch08_shifted_inverse_iteration.m
%
% Find an eigenpair near a chosen shift by inverse iteration.
%
% Origin:
%   MATLAB translation of ch08_shifted_inverse_iteration.f90 by
%   Alexander Godunov. Each step solves (A-shift*I)*z=y, followed by
%   y=z/norm(z). Pivoted LU factors are computed once and reused.
%
% Example:
%   A = [ 1  2  3; 2  2 -2; 3 -2  4 ], shift = 3.4.
%   Its eigenvalues are approximately -2.54138127, 3.54138127,
%   and 6.00000000. The nearest eigenvalue to 3.4 is 3.54138127.
%
% A shift at an eigenvalue makes A-shift*I singular. A nearby shift
% can be ill-conditioned. The initial vector must have a component
% along the desired eigenvector.
%=======================================================================

clear; clc;

A = [1.0,  2.0,  3.0;
     2.0,  2.0, -2.0;
     3.0, -2.0,  4.0];
y = [1.0; 1.0; 1.0];
shift = 3.4;
tol = 1.0e-10;
max_iter = 100;

fprintf('Shifted inverse iteration for one eigenpair\n');
fprintf('\nMatrix A:\n');
for i = 1:size(A,1)
    fprintf('%12.6f%12.6f%12.6f\n', A(i,:));
end
fprintf('\nInitial vector:%12.6f%12.6f%12.6f\n', y);
fprintf('Shift = %12.6f\n', shift);
fprintf('Eigenpair residual tolerance = %.4e\n', tol);

[y, lambda, iterations, residual, pivot_step, info] = ...
    shifted_inverse(A, y, shift, tol, max_iter);
if info == 1
    error('Invalid input: check symmetry, initial vector, and controls.');
elseif info == 2
    error(['A-shift*I is singular or nearly singular at pivot step %d. ' ...
           'Choose a different shift.'], pivot_step);
elseif info == 3
    error('The inverse iteration vector is zero; cannot normalize.');
end

fprintf('\nEigenvalue near shift = %12.6f\n', lambda);
fprintf('Unit eigenvector:\n');
for i = 1:numel(y)
    fprintf('y(%d) = %12.6f\n', i, y(i));
end
fprintf('Iterations = %d\n', iterations);
fprintf('Eigenpair residual ||A*y-lambda*y|| = %.4e\n', residual);
if info == 4
    error('Iteration limit reached before the tolerance was met.');
end


function [y, lambda, iterations, residual, pivot_step, info] = ...
    shifted_inverse(A, y, shift, tol, max_iter)
%=======================================================================
% Approximate the eigenvalue of a real symmetric A nearest shift.
%
% Input:
%   A        - square symmetric matrix, left unchanged
%   y        - nonzero initial vector, returned as a unit vector
%   shift    - fixed estimate of the desired eigenvalue
%   tol      - positive tolerance for norm(A*y-lambda*y)
%   max_iter - maximum number of inverse iterations
%
% Output:
%   lambda     - signed Rayleigh quotient y'*A*y / (y'*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. Solve at each step without
% forming an inverse. Use the ORIGINAL A to compute the Rayleigh
% quotient and residual, and check the full eigenpair for convergence.
%=======================================================================

n = numel(y);
lambda = 0.0;
iterations = 0;
residual = Inf;
pivot_step = 0;
info = 1;
if n < 1 || ~isequal(size(A), [n,n]) || ~isreal(A) || ~isreal(y) ...
        || any(any(A ~= A.')) || ~isscalar(shift) || ~isfinite(shift) ...
        || ~isscalar(tol) || ~(tol > 0) || ~isscalar(max_iter) ...
        || max_iter < 1 || max_iter ~= floor(max_iter)
    return
end
y = y(:);
norm_y = norm(y);
if norm_y == 0.0
    return
end
y = y/norm_y;

% Subtract the fixed shift; retain A for the residual and quotient.
LU = A;
for i = 1:n
    LU(i,i) = LU(i,i)-shift;
end
[LU, ipiv, pivot_step] = lu_factor(LU);
if pivot_step ~= 0
    info = 2;
    return
end

for k = 1:max_iter
    % Reuse the saved factors for a different right-hand side.
    z = lu_solve(LU, ipiv, y);
    norm_z = norm(z);
    if norm_z == 0.0
        info = 3;
        return
    end
    y = z/norm_z;

    Ay = A*y;
    lambda = y'*Ay;
    residual = norm(Ay-lambda*y);
    iterations = k;
    if residual <= tol
        info = 0;
        return
    end
end
info = 4;
end


function [LU, ipiv, pivot_step] = lu_factor(LU)
%=======================================================================
% Factor B in place as P*B=L*U with partial row pivoting.
%
% Entries below the diagonal hold L multipliers; the diagonal and
% upper entries hold U. ipiv(k) records the row swapped with k.
% Exchange entire rows, including previously computed multipliers.
% Report a zero or nearly zero pivot relative to the input scale.
%=======================================================================

n = size(LU,1);
ipiv = zeros(n,1);
pivot_step = 0;
pivot_tol = 10.0*n*eps(1.0)*max(abs(LU(:)));
for k = 1:n
    pivot_row = k;
    best = abs(LU(k,k));
    for i = k+1:n
        if abs(LU(i,k)) > best
            best = abs(LU(i,k));
            pivot_row = i;
        end
    end
    ipiv(k) = pivot_row;
    if best <= pivot_tol
        pivot_step = k;
        return
    end

    if pivot_row ~= k
        for j = 1:n
            store = LU(k,j);
            LU(k,j) = LU(pivot_row,j);
            LU(pivot_row,j) = store;
        end
    end

    for i = k+1:n
        LU(i,k) = LU(i,k)/LU(k,k);
        for j = k+1:n
            LU(i,j) = LU(i,j)-LU(i,k)*LU(k,j);
        end
    end
end
end


function z = lu_solve(LU, ipiv, b)
%-----------------------------------------------------------------------
% Apply saved row swaps to b, then solve L*w=P*b and U*z=w.
%-----------------------------------------------------------------------
n = numel(b);
z = b;
for i = 1:n
    if ipiv(i) ~= i
        store = z(i);
        z(i) = z(ipiv(i));
        z(ipiv(i)) = store;
    end
end
for i = 1:n
    for j = 1:i-1
        z(i) = z(i)-LU(i,j)*z(j);
    end
end
for i = n:-1:1
    for j = i+1:n
        z(i) = z(i)-LU(i,j)*z(j);
    end
    z(i) = z(i)/LU(i,i);
end
end