function [x,flag,relres,iter] = fgmres_mgs(A,b,restart,tol,maxit,M,x)
%FGMRES_MGS: flexible (right) preconditioned generalized minimum
%            residual  method with restarts 
%            using modified Gram-Schmidt-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_mgs: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
% ----------------------------------------------------------------------------
V=zeros(n,inner);       % Arnoldi vectors
Z=zeros(n,inner);       % Basis vectors for reconstructing x
cs=zeros(inner,1);      % Givens rotation cosines/sines
sn=zeros(inner,1); 
H =zeros(inner+1,inner);% Hessenberg m-x
g =zeros(inner+1,1);    % Right hand side of the least-squares problem
% ----------------------------------------------------------------------------
% Outer loop
% ----------------------------------------------------------------------------
for out_iter=1:outer
    beta   = normr;
    V(:,1) = r/normr;
    g(1)  = beta;
    % ------------------------------------------------------------------------
    % Inner loop
    % ------------------------------------------------------------------------
    for inn_iter=1:inner
        % construct the new basis vector
        Z(:,inn_iter) = Mfun(V(:,inn_iter));
        %  compute v=A*v
        w = Afun(Z(:,inn_iter));
        % orthogonalize to the previous vectors
        for k = 1:inn_iter
            H(k,inn_iter)= w'*V(:,k);
            w = w - H(k,inn_iter)*V(:,k);
        end
        H(inn_iter+1,inn_iter) = norm(w);
        if inn_iter < inner
            V(:,inn_iter+1) = w/H(inn_iter+1,inn_iter);
        end
        % apply all previous Givens rotations to the new column of H
        for k = 1:inn_iter-1,
            H(k:k+1,inn_iter)  = rot_app(cs(k),sn(k),H(k:k+1,inn_iter));
        end
        if inn_iter < n
            %  Compute new Given's rotation
            [cs(inn_iter),sn(inn_iter)]=rot_mat(H(inn_iter,inn_iter),H(inn_iter+1,inn_iter));
            % 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 H
            H(inn_iter:inn_iter+1,inn_iter) = rot_app(cs(inn_iter),sn(inn_iter),H(inn_iter:inn_iter+1,inn_iter));
            H(inn_iter+1,inn_iter)          = 0.0;
        end
        % 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 = H(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);
