function [x, res, iter, flag] = mygmres( A, b, Ml, Mr, varargin ) 

%
% GMRES:
%    solves the linear system Ax=b using the
%    Generalized Minimal residual ( GMRESm ) method with restarts .
%
% Usage:
%
% [x, res, iter, flag] = mygmres( A, b, Ml, Mr ) 
% [x, res, iter, flag] = mygmres( A, b, Ml, Mr, x ) 
% [x, res, iter, flag] = mygmres( A, b, Ml, Mr, x, options )   
%
%
%  Input parameters
%
%  A       the system matrix A.
%          is either a real(n,n) matrix or
%          the name of a function that computes y = A * x.
%          The calling sequence of this function is [y] = Ax(x).
%
%  b       real(n)
%          the right-hand side. 
%
%  Ml      the left preconditioner
%          is either an invertible n by n matrix or the
%          name of a function that solves Ml y = x.
%          The calling sequence of this function is  [y] = Ml(x).
%          No left preconditioning is applied when Ml = [].
%
%  Mr      the right preconditioner
%          is either an invertible n by n matrix or the
%          name of a function that solves Mr y = x.
%          The calling sequence of this function is  [y] = Mr(x).
%          No right preconditioning is applied when Mr = [].
%
%  x       real(n)
%          the initial approximate solution. Default x = 0.
%
%  options algorithm options
%          options.tol    stopping tolerance (Default tol = 1.e-7)
%                         the algorithm attempts to generate x such that   
%                         || Ml^(-1) ( b-Ax ) || <= tol
%          options.maxit   maximum number of iterations (Default maxit = 100)
%          options.restart GMRES is restarted after restart iteratons 
%                          (Default restart = 20)
%           
%           
%
%
%  Output parameters:
%
%  x       Final approximate solution.
%
%  flag    integer
%          Error flag, as follows:
%              0 => normal termination; desired tolerance met.
%              1 => desired tolerance not met after maxit iterations.
%
%  res     real
%          vector of residual norms.
%          res(i+1) is the residual norm of iteration i.
%
%  iter    integer
%          contains the total number of iterations in GMRES.
%          the number of  matrix vector products is given by  iter + 1.
%
%
%  M. Heinkenschloss
%  February 3, 2009
%  

%% set algorithm parameters
if( nargin < 5 );  
    x = zeros(size(b,1),1); 
else
    x = varargin{1};
end
if( nargin < 6 );  
    options = []; 
else
    options = varargin{2};
end
maxit   = 100;
tol     = 1.e-7;
restart = 20;
if( ~isempty(options) )
    if( isfield(options,'maxit') );    maxit   = options.maxit; end
    if( isfield(options,'restart') );  restart = options.restart; end
    if( isfield(options,'tol') );      tol     = options.tol; end
end

%% Initialization
iter = 0;
flag = 0;

% compute initial residual
if( isstr(A) )
    r = b - feval( A, x );
else
    r = b - A*x;
end
% apply left preconditioner
if( isstr(Ml) )
    r = feval( Ml, r );
elseif( ~isempty(Ml) )
    r = Ml\r;
end

resnrm = norm( r );
res(1) = resnrm;
if ( res(1) < tol ) return, end

% initialize workspace
n = size(x(:),1);
m = restart;
V(1:n,1:m+1) = zeros(n,m+1);
H(1:m+1,1:m) = zeros(m+1,m);
cs(1:m)      = zeros(m,1);
sn(1:m)      = zeros(m,1);
e1           = zeros(n,1);
e1(1)        = 1.0;

%% Begin iteration
while ( resnrm > tol & iter < maxit )
  
    V(:,1) = r(:) / norm( r );
    s = norm( r )*e1;
    for i = 1:m
        iter = iter+1;
        
        % apply right preconditioner    Mr^(-1) V(i)
        if( isstr(Mr) )
            w = feval( Mr, V(:,i) );
        elseif( ~isempty(Mr) )
            w = Mr\V(:,i);
        else
            w = V(:,i);
        end
        % A*Mr^(-1) V(i)
        if( isstr(A) )
            w = feval( A, w );
        else
            w = A*w;
        end
        % apply left preconditioner  Ml^(-1)A*Mr^(-1) V(i)
        if( isstr(Ml) )
            w = feval( Ml, w );
        elseif( ~isempty(Ml) )
            w = Ml\w;
        end
        
        % orthogonalize
        for k = 1:i,
            H(k,i)= w(:)'*V(:,k);
            w = w - H(k,i)*V(:,k);
        end
        H(i+1,i) = norm( w );
        V(:,i+1) = w(:) / H(i+1,i);
        % apply Givens rotation
        for k = 1:i-1,
            temp     =  cs(k)*H(k,i) + sn(k)*H(k+1,i);
            H(k+1,i) = -sn(k)*H(k,i) + cs(k)*H(k+1,i);
            H(k,i)   = temp;
        end
        % form i-th rotation matrix
        [cs(i),sn(i)] = giv_rot( H(i,i), H(i+1,i) );
        % approximate residual norm
        temp        = cs(i)*s(i);
        s(i+1)      = -sn(i)*s(i);
        s(i)        = temp;
        H(i,i)      = cs(i)*H(i,i) + sn(i)*H(i+1,i);
        H(i+1,i)    = 0.0;
        resnrm      = abs(s(i+1));
        res(iter+1) = resnrm;
        % update approximation and exit
        if ( resnrm <= tol | iter > maxit )
            y = H(1:i,1:i) \ s(1:i);
            z = V(:,1:i)*y;
            if( isstr(Mr) )
                z = feval( Mr, z );
            elseif( ~isempty(Mr) )
                z = Mr\z;
            end
            
            x = x + z;
            if ( iter > maxit ); flag = 1; end
            break;
        end
    end
    
    if ( resnrm <= tol | iter > maxit ), break, end
    % restart
    % update approximation
    y = H(1:m,1:m) \ s(1:m);
    z = V(:,1:m)*y;
    if( isstr(Mr) )
        z = feval( Mr, z );
    elseif( ~isempty(Mr) )
        z = Mr\z;
    end
    
    x = x + z;
    
    % compute initial residual
    if( isstr(A) )
        r = b - feval( A, x );
    else
        r = b - A*x;
    end
    % apply left preconditioner
    if( isstr(Ml) )
        r = feval( Ml, r );
    elseif( ~isempty(Ml) )
        r = Ml\r;
    end
    
    resnrm      = norm(r);
    res(iter+1) = resnrm;
end

if ( resnrm > tol ) flag = 1; end;

end % END of mygmres.m


function [ c, s ] = giv_rot( a, b )

% GIV_ROT
%     Computes the Givens rotation matrix parameters for a and b, 
%     i.e. GIV_ROT computes 2x2 matrix such that
%
%       ( c  s ) ( a )  = ( sqrt(a^2 + b^2) )
%       ( s -c ) ( b )    (       0         )
%     or
%       ( a  b ) ( c  s ) = ( sqrt(a^2 + b^2)    0 )
%                ( s -c )   
%
%     and  s^2 + c^2 = 1.

   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
end

