function [x,flag,relres,iter] = fgmres_h(A,b,restart,tol,maxit,M,x)
%FGMRES_H: flexible (right) preconditioned generalized minimum
%          residual  method with restarts 
%          using Householder reflections-based Arnoldi iteration
%
%  Input/output:  same as for gmres (note: only one preconditioner)
%


% ----------------------------------------------------------------------------
% Check parameters first
% Warning: the check is not nearly as thorough as Matlab's
% ----------------------------------------------------------------------------
if nargin < 2
    error('gmres_h:parameters','Too few parameters');
end
if isa(A,'float')
    Afun = @(v) A*v;
else
    Afun = A;
end
n = length(b);
if (nargin < 3) || isempty(restart) || (restart >= n)
    restart = n;
end
if (nargin < 4) || isempty(tol)
    tol = 1.0e-6;
end
if (nargin < 5) || isempty(maxit)
    if restart<n
        maxit = min(ceil(n/restart),10);
    else
        maxit = min(n,10);
    end
end
if restart<n
    outer = maxit;
else
    outer = 1;
end
inner = restart;
if ((nargin < 6) || isempty(M))
    Mfun = @(v) v;
else
    if isa(M,'float')
        Mfun = @(v) M\v;
    else
        Mfun = M;
    end
end
if ((nargin < 7) || isempty(x))
    x = zeros(n,1);
end
% ----------------------------------------------------------------------------
% end parameter check
% ----------------------------------------------------------------------------


% ----------------------------------------------------------------------------
% Initialization
% ----------------------------------------------------------------------------
bnrm2 = norm(b);
if (bnrm2==0.0)
    bnrm2 = 1.0;
end
r = b-Afun(x);
normr  = norm(r);
relres = normr/bnrm2;
if relres<tol
    flag = 0;
    return;
end
flag = 1;
iter = [0, 0];

% ----------------------------------------------------------------------------
% Preallocate storage
% ----------------------------------------------------------------------------
W=zeros(n,inner);       % Householder reflection vectors
Z=zeros(n,inner);       % Basis vectors for reconstructing x
cs=zeros(inner,1);      % Givens rotation cosines/sines
sn=zeros(inner,1); 
R =zeros(inner,inner);  % "R" part of the QR factorization of
                        % Hessenberg m-x H
g =zeros(inner+1,1);    % Right hand side of the least-squares problem
% ----------------------------------------------------------------------------
% Outer loop
% ----------------------------------------------------------------------------
for out_iter=1:outer
    %  Construct w for the first Householder reflector
    %  w = r + sign(r(1))*||r||*e1
    [W(:,1),~,beta]=householder_vec(r,1);
    % g  := (I-2*w*w')r
    g(1)  = -beta;
    % ------------------------------------------------------------------------
    % Inner loop
    % ------------------------------------------------------------------------
    for inn_iter=1:inner
        %  Form P1*P2*P3...Pj*ej.
        %  v = Pj*ej = ej - 2*w*w'*ej
        v           = -2.0*(W(inn_iter,inn_iter)')*W(:,inn_iter);
        v(inn_iter) = v(inn_iter) + 1.0;
        %  v = P1*P2*...Pj-1*(Pj*ej)
        for k = (inn_iter-1):-1:1
            v = householder_app(W(:,k),v);
        end
        %  Explicitly normalize v to reduce the effects of round-off.
        v = v/norm(v);
        %  compute v=M*v - need to store these because of flexible preconditioning
        v = Mfun(v);
        Z(:,inn_iter) = v;
        %  compute v=A*v
        v = Afun(v);
        %  Form Pj*Pj-1*...P1*AMv.
        for k = 1:inn_iter
            v = householder_app(W(:,k),v);
        end
        %  Determine Pj+1.  Note: when inn_iter=n this is no longer necessary
        %  Construct w for Householder reflector Pj+1 and apply
        %  it to v
        if inn_iter < inner
            [W(:,inn_iter+1),v,~]=householder_vec(v,inn_iter+1);
        else
            [~,v,~]              =householder_vec(v,inn_iter+1);
        end
        %  Apply all previous Given's rotations to the newly formed v
        for k = 1:inn_iter-1,
            v(k:k+1)  = rot_app(cs(k),sn(k),v(k:k+1));
        end
        if inn_iter < n
            %  Compute new Given's rotation
            [cs(inn_iter),sn(inn_iter)]=rot_mat(v(inn_iter),v(inn_iter+1));
            % apply it to g
            g(inn_iter:inn_iter+1) = rot_app(cs(inn_iter),sn(inn_iter),[g(inn_iter);0.0]);
            % apply it to v
            v(inn_iter:inn_iter+1) = rot_app(cs(inn_iter),sn(inn_iter),v(inn_iter:inn_iter+1));
            v(inn_iter+1)          = 0.0;
        end
        R(1:inn_iter,inn_iter) = v(1:inn_iter);
        % compute the relative residual
        relres = abs(g(inn_iter+1)) / bnrm2;
        if relres < tol,
            % Converged!
            break;
        end
    end
    % ------------------------------------------------------------------------
    % end inner loop
    % ------------------------------------------------------------------------
    % solve the least squares system
    y = R(1:inn_iter,1:inn_iter) \ g(1:inn_iter);
    % update the solution
    x = x + Z(:,1:inn_iter)*y;
    r = b-Afun(x);
    normr  = norm(r);
    relres = normr/bnrm2;
    if relres < tol
        % Converged!
        flag = 0;
        break;
    end
end
% ----------------------------------------------------------------------------
% end outer loop
% ----------------------------------------------------------------------------
iter = [out_iter, inn_iter];


% ----------------------------------------------------------------------------
% Compute the Givens rotation matrix parameters for a and b.
% ----------------------------------------------------------------------------
function [c,s] = rot_mat(a,b)
if b == 0.0
    c = 1.0;
    s = 0.0;
elseif abs(b) > abs(a)
    temp = a / b;
    s = 1.0 / sqrt( 1.0 + temp^2 );
    c = temp * s;
else
    temp = b / a;
    c = 1.0 / sqrt( 1.0 + temp^2 );
    s = temp * c;
end
% ----------------------------------------------------------------------------
% Apply the Givens rotation matrix to a 2D vector v
% ----------------------------------------------------------------------------
function [w] = rot_app(a,b,v)
w(1) =  a*v(1) + b*v(2);
w(2) = -b*v(1) + a*v(2);
% ----------------------------------------------------------------------------
% Sign function
% ----------------------------------------------------------------------------
function sgn = sign1(x)
sgn = sign(x);
if (sgn == 0)
    sgn = 1;
end
% ----------------------------------------------------------------------------
% Construct Householder reflection vector, acting from dimension j
% Apply it to v
% ----------------------------------------------------------------------------
function [w,v,alpha]=householder_vec(v,j)
w=[zeros(j-1,1);v(j:end)];
alpha = norm(v(j:end));
if alpha ~= 0.0
    alpha = sign1(v(j))*alpha;
    w(j)  = w(j) + alpha;
    w     = w / norm(w);
    v(j+1:end) = 0; 
    v(j)       = -alpha;
end
% ----------------------------------------------------------------------------
% Apply Householder reflection to v
% ----------------------------------------------------------------------------
function [v]=householder_app(w,v)
v = v - (2.0*(w'*v))*w;
