%% gmrest_k.m --- Illustration of Preconditioned GMRES(k) -- Chapter 3
%%
%%       Solve: P2*A*P1*y = P2*b for x=P1*y
%%
%% Usage: [x,r]=gmrest_k(A,b,k,P2,P1,tol,maxit,x0)  for  P2*A*P1*y = P2*b
%%    or      x=gmrest_k(A,b,k,P2,[],tol,maxit,x0)  for  P2*A*x    = P2*b
%%    or      x=gmrest_k(A,b,k,[],P1,tol,maxit,x0)  for     A*P1*y = b
%%    or      x=gmrest_k(A,b,k,[],[],tol,maxit,x0)  for     A*x    = b
%%    or      x=gmrest_k(A,b,k)
%%    or      x=gmrest_k(A,b)    % where r is the residual history vector
%% Sample data:  rand('seed',3);  n=16,
%                A=rand(n)+2*eye(n); P2=diag(1./diag(A)); x=(n:-1:1)'; b=A*x;
%                x=gmrest_k(A,b,10,P2);

function [x,res]=gmrest_k(A,b,k, P2,P1, tol,maxit,x0)
if nargin<2, help gmrest_k; return; end
   [m n]=size(A);   if m~=n, error('Check dimension of A!'); end
if nargin<3,  k=min(5,m); end
if nargin<4,  P2=[];      end
if nargin<5,  P1=[];      end
if nargin<6,  tol=1.0e-4; end %=========================%
if nargin<7,     maxit=m; end
if nargin<8, x0=zeros(m,1); end,    Left='P2*';Right='*P1';
      if size(P2,1)==0,P2=speye(m); Left=' ';  end
      if size(P1,1)==0,P1=speye(m); Right=' '; end
x=x0;res=[];
     r = b - A*x;  norm_0=norm(r); iter=0; ok=0; res(1)=norm_0;

while iter<maxit

% Step (1)
          x0 = x;  r = b - A*x;   r=P2*r;
          r2=norm(r); Q=[];R=[];c=[];s=[];
% Step (2)
          q1 = r/r2; Q=q1;   rhs=zeros(m,1);rhs(1)=r2;
   for i=1:k
% Step (3)
          w = Q(:,i);
          w = P1*w;
          w =  A*w;
          w = P2*w;
% Step (4)
   for L=1:i
     R(L,i) = w'*Q(:,L);  w=w-R(L,i)*Q(:,L);
   end 
% Step (5)
   R(i+1,i) = norm(w);   Q(:,i+1)=w/R(i+1,i);
% Step (6) 
   for L=1:(i-1)
     t=c(L)*R(L,i) + s(L)*R(L+1,i);
     R(L+1,i)=s(L)*R(L,i) - c(L)*R(L+1,i);
     R(L,i)=t;
   end
% Step (7)
   if abs(R(i+1,i))<1.0e-16, c(i)=1; s(i)=0;
   elseif abs(R(i+1,i))>abs(R(i,i))
      t=R(i,i)/R(i+1,i); s(i)=1/sqrt(1+t^2);  c(i)=s(i)*t; else
      t=R(i+1,i)/R(i,i); c(i)=1/sqrt(1+t^2);  s(i)=c(i)*t; 
   end
% Step (8)
   t=c(i)*rhs(i);  rhs(i+1)=s(i)*rhs(i);  rhs(i)=t;
   R(i,i)=c(i)*R(i,i) + s(i)*R(i+1,i);
   R(i+1,i) = 0; 
% Step (9)
   y = R(1:i,1:i) \ rhs(1:i,1);
% Step (10)
          w = Q(:,1:i)*y;
          w = P1*w;
   x = x0 + w;
% Step (11)
   r=b-A*x;   r2=norm(r); res=[res r2];  r2=r2/norm_0;  
if m<25
   fprintf('%s iter=%3d, res=%e\n',mfilename,iter*k+i,r2)
end
   if r2 <= tol, ok=1; break; disp('gmres_k done...'); end
% Step (12)
end % i
   if ok==1, break; end
   iter=iter+1;
end %while
   if iter>=maxit, iter=iter-1; end
fprintf('%s for [%sA%s]: Total GMREST(%d) Steps = %d to achieve %e [N=%d]\n',...
         mfilename, Left,Right, k, iter*k+i, norm(r)/norm_0, m);
