function [sol,nkryv,res_L2,sol_L2,info] = gcrot(m,k,maxiter, b, sol,restol, ...
matvec, precond)
% -----------------------------------------------------------------------
% Purpose: Solves the linear system using GCROT with FGMRES as the nested
%   Krylov solver.  Attempts to solve the linear system A*sol = b to the 
%   tolerance restol, where the matrix A is implicitly defined by the 
%   subroutine matvec.
% 
% Notes: This is a variant of de Sturler's GCROT algorithm.  First,
%   FGMRES is used as the nested solver.  Second, no residual error 
%   analysis is used for truncation.  Instead, the oldest vectors are
%   discarded, in analogy with LGMRES(m,k).
% 
% Inputs:
%   m = maximum number of subspace vectors stored (m + k in paper)
%   k = maxium size of the augmented subspace
%   maxiter = maximum number of matrix-vector products
%   b = local right-hand side vector
%   restol = tolerance with with to solve the system
%   matvec = subroutine defining the matrix-vector product
%   precond = subroutine defining precondtitioning
% 
% InOuts:
%   sol = local solution vector and initial guess
% 
% Outputs:
%   nkryv = number of matvecs used to solve 
%   info = integer that indicates the exit condition
%   res_L2 (optional) = final residual scaled using Rscl
%   sol_L2 (optional) = the L2 norm of the solution update
% 
% -----------------------------------------------------------------------
if (nargin ~= 8)
    error('MATLAB:gcrot:NumInputs','Wrong input arguments.');
end

% assign nvar and check consistency of sol and b
nvar = size(b,1);
if (size(sol,1) ~= nvar) 
    error('MATLAB:gcrot:Inputs','size of sol is incompatible with b');
end

% assume normal exit
info = 0;

% check m and maxiter
if ((m < 1) || (maxiter < 1))
    error('MATLAB:gcrot:Inputs','illegal m/maxiter value');
end

miniter = 1;

% allocate arrays
C = zeros(nvar,m+1);    % subspace that residual is orthogonal to
U = zeros(nvar,m);      % subspace used to build solution
H = zeros(m+1,m);       % rectangular matrix for H_bar and B matrix
g = zeros(m+1,1);       % rhs of reduced system
x = zeros(m+1,1);       % solution of reduced system
sn = zeros(m+1,1);      % vector of Givens sines 
cs = zeros(m+1,1);      % vector of Givens cosines
r = zeros(nvar,1);      % current residual vector
cnew = zeros(nvar,1);   % residual update vector
unew = zeros(nvar,1);   % solution update vector

% assume an initial guess of zero, and set initial residual
sol(:) = 0;
r(:) = b(:);
nkryv = 0;

% get the norm of the initial residual
beta = norm(r);

if (beta < 0)
    error('MATLAB:gcrot:Residual','problem with residual');
end

if(beta < eps)
    %  system is already solved
    disp('solved already');
    clear all;
    return;
end

norm0 = beta;

%  outer, GCROT, loop
jmax = m;
kmax = 0; 
outdone = 0; % on first iteration, we use FGMRES(m)

for l = 1 : maxiter     %gcroloop
    
    %---------------------------------
    % begin nested FGMRES(jmax)
    
    % normalize residual to get W_{1} = C(:,kmax+1)
    tmp = 1/beta;
    C(:,kmax+1) = r(:)*tmp;
    
    % initialize the rhs and lhs of the reduced system
    H(:,:) = 0;
    g(:) = 0;
    g(1) = beta;

    % inner, FGMRES, loop
    indone = 0;
    for j = 1 : jmax % fgmres loop
        nkryv = nkryv + 1;
                 
        % precondition the vector W_{j} = C(:,kmax+j)
        U(:,kmax+j) = precond(C(:,kmax+j));
            
        % add to Krylov subspace
        C(:,kmax+j+1) = matvec(U(:,kmax+j));
          
        % orthogonalize W_{j+1} against C(:,1:kmax) and  
        % W_{1},W_{2},...,W_{j} = C(:,kmax+1:kmax+j)
        [H(:,j),C(:,:),gsinfo] = modGramSchmidt(kmax+j+1,H(:,j),C(:,:));
                
        if (gsinfo~=0)
                error('MATLAB:gcrot:modGramSchmidt', 'modGramSchmidt exited with info = ',ginfo);
                info = -gsinfo;
                return;
        end
        
        % apply old Givens rotations to new column of the Hessenberg
        % matrix
        for i = 1:j-1
            [H(kmax+i,j),H(kmax+i+1,j)] = applyGivens(sn(i),cs(i),...
                                        H(kmax+i,j),H(kmax+i+1,j));
        end
        
        % generate the new Givens rotation matrix, and apply it to the
        % last two elements of H(:,i) and g
        [H(kmax+j,j),H(kmax+j+1,j),sn(j),cs(j)] = ...
            generateGivens(H(kmax+j,j),H(kmax+j+1,j));
        [g(j),g(j+1)] = applyGivens(sn(j),cs(j),g(j),g(j+1));
   
        beta = abs(g(j+1));
        
        % check if solution is converged, or max iterations reached
        % and output residual
        if (((beta < norm0*restol) && (j>=miniter))|| (j==jmax) || ...
                (nkryv >= maxiter))
            indone = 1;
        end
        
        if(indone)
            break;
        end
    end % end inner fgmres loop
    
    % calculate U_new = (Z_m - U_k B_m) R^{-1} g
    % solve to get x = R^{-1} g
    x(1:j) = g(1:j);
    for i = j:-1:1
        x(i) = x(i)/H(kmax+i,i);
        for i2 = i-1:-1:1
            x(i2) = x(i2) - H(kmax+i2,i)*x(i);
        end
    end
    unew(:) = 0;
    for i= 1: j
        unew(:) = unew(:) + x(i)*U(:,kmax+i);
    end
    
    % update U_new = U_new - U_k B_m x
    for i2 = 1:kmax
        tmp = 0;
        for i = 1:j
           tmp = tmp + H(i2,i)*x(i);
        end
        unew(:) = unew(:) - tmp*U(:,i2);
    end
    
    % finished with g, so undo rotations to find C_new
    x(:) = g(:);
    x(j+1) = 0;
    for i = j:-1:1
        [x(i),x(i+1)] = applyGivens(-sn(i),cs(i),x(i),x(i+1));
    end
    cnew(:) = 0;
    
    for i = 1:j+1
        cnew(:) = cnew(:) + x(i)*C(:,kmax+i);
    end
    
    
    % normalize and scale new vectors and update solution and res
    alpha = 1/norm(cnew);
    cnew(:) = alpha*cnew(:);
    unew(:) = alpha*unew(:);    
    alpha = dot(cnew,r);
    r(:) = r(:) - alpha*cnew(:);
    
    sol(:) = sol(:) + alpha*unew(:);
     
    % get new residual norm
    % should be the same as last norm in FGMRES!!!
    beta = norm(r);
    
    % check if solution is converged, or max iterations reached
    % and output residual
    if (((beta < norm0*restol) && (l>=miniter)) || ...
         (nkryv >= maxiter))
     outdone = 1;
    end
    
    if(outdone) 
        break; 
    end
    
    % determine which vector to discard from C(:,1:kmax) and
    %  U(:,1:kmax), if necessary
    if (kmax<k)
         kmax = kmax + 1;
         jmax = jmax - 1;
         ptr = kmax;
    else
       if (k > 0) 
          ptr = rem(ptr,kmax) + 1; 
       end
    end
    
    if (k==0) 
        continue 
    end
    
    C(:,ptr) = cnew(:);
    U(:,ptr) = unew(:);    
end %end gcroloop

% recalculate the final residual (check)
r = matvec(sol);
r(:) = b(:) - r(:);

% get the norm of the final residual
res_L2 = norm(r);
sol_L2 = norm(sol);

        
function [Hsbg,w,info] = modGramSchmidt(i,Hsbg,w)   %Subfunction
% -----------------------------------------------------------------------
% Purpose: Modified Gram-Schmidt orthogonalization procedure; the ith 
%   vector of w(:,1:i) is orthogonalized against the previous i-1
%   vectors.
% 
% Notes: Re-orthogonalization is performed if the cosine of the angle
%   between w(:,i) and w(:,k) (k < i) is greater than 0.98.  The norm of
%   the "new" vector is kept in nrm0 and updated after operating with
%   each vector
%
% Inputs:
%   i = the index of the vector in w being orthogonalized
%
% InOuts:
%   Hsbg = the column of the upper Hessenburg matrix being updated
%   w(:,1:i) = the ith vector of w is orthogonalized against the first
%              i-1 vectors
% 
% Outputs:
%   info = integer that indicates the exit condition of the subroutine
%        = 0, if successful (no problems)
%        = 1, if the norm of w(:,i) < 0
%        = 2, if the norm of w(:,i) is NaN
%        = 3, if w(:,i) is a linear combination of w(:,1:i-1)
% 
% Pre: 1) the vectors w(:,1:i-1) are orthonormal
% Post: 2) the vectors w(:,1:i) are orthonormal
%
% Author: Based on Kesheng John Wu's mgsro subroutine in Saad's SPARSKIT
% -----------------------------------------------------------------------
reorth = 0.98;

% assume normal exit
info = 0;
nrm0 = dot(w(:,i),w(:,i));     %norm squared of w (preconditioned residual)
thr = nrm0*reorth;  %threshold

if (nrm0 <= 0)
    % the norm of w(:,i) < 0
    info = 1;
    return
elseif (~(nrm0 > 0) && (1/nrm0 > 0))
    % the norm of w(:,i) is NaN
    info = 2;
    return
end

% begin main Gram-Schmidt loop
for k= 1 : i-1  
    
    fct = dot(w(:,i),w(:,k));    
    Hsbg(k) = fct;
    w(:,i) = w(:,i) - fct*w(:,k);
    
    %  check if re-orthogonalization is necessary
    if (fct*fct > thr)
        fct = dot(w(:,i),w(:,k));
        Hsbg(k) = Hsbg(k) + fct;
        w(:,i) = w(:,i) - fct*w(:,k);
    end
    
    % update the norm and check its size
    nrm0 = nrm0 - Hsbg(k)*Hsbg(k);
    if(nrm0 < 0) 
        nrm0 = 0; 
    end
    thr = nrm0*reorth;
end


nrm1 = norm(w(:,i));
Hsbg(i) = nrm1;
if (nrm1 <= 0)
    %  w(:,i) is a linear combination of w(:,1:i-1)
    info = 3;
    return
end

%scale the resulting vector
fct = 1/nrm1;
w(:,i) = w(:,i)*fct;

function [h1,h2] = applyGivens(s,c,h1,h2)   %Subfunction
% -----------------------------------------------------------------------
% Purpose: applies the Givens rotation defined by c and s to h1, h2
% -----------------------------------------------------------------------
temp = c*h1 + s*h2;
h2 = c*h2 - s*h1;
h1 = temp;

function [dx,dy,s,c] = generateGivens(dx,dy) %Subfunction
% -----------------------------------------------------------------------
% Purpose: generates the Givens rotation matrix, defined by s and c,
%   using dx and dy to define the angle to eliminate.  The values of dx
%   and dy are updated to reflect the applied rotation
% 
% Notes: based on givens() of SPARSKIT, which is based on p.202 of
%   "Matrix Computation" by Golub and van Loan
% -----------------------------------------------------------------------
if ((dx==0) && (dy==0))
    c = 1;
    s = 1;
elseif (abs(dy) > abs(dx))
    t = dx/dy;
    dx = sqrt(1 + t*t);
    s = (1/dx)*(dy/abs(dy));
    c = t*s;
elseif (abs(dy) <= abs(dx))
    t = dy/dx;
    dy = sqrt(1 + t*t);
    c = (1/dy)*(dx/abs(dx));
    s = t*c;
else
    % dx and/or dy invalid
    dx = 0;
    dy = 0;
    c = 1;
    s = 0;
end
dx = abs(dx*dy);
dy = 0;