function pcgdemo2(p)
%PCGDEMO2  demonstration of various CG-related iterations
%
% A simple demonstration comparing the convergence of
% five iterative methods: steepest descents, conjugate gradients,
% diagonally preconditioned conjugate gradients, conjugate
% gradients preconditioned with incomplete Cholesky factorization,
% and conjugate gradients preconditions with multigrid, all applied
% to the 5 point Laplacian.
%
% mesh size is h = 1/2^p, where p is the argument (defaults to 7)
% Douglas N. Arnold, 2009-10-15

if nargin == 0
  p=4;
end

n=2^p-1;
A = gallery('poisson',n);
b=ones(n*n,1);
clf

% number of iterations
niter = 99;

%%% Steepest descent
% initial guess
x=zeros(n*n,1);
% store norms of residuals% steepest descents
ressd=zeros(niter+1,1);

r=b-A*x;
r2 = r'*r;
ressd(1)=sqrt(r2);
for i = 1:niter
  p = A*r;
  lambda = r2/(r'*p);
  x = x + lambda*r;
  r = r - lambda*p;
  r2 = r'*r;
%  fprintf('iteration %3d   x(1)  %20.18f\n',i,x(1))
  ressd(i+1)=sqrt(r2);
end

%%% Conjugate gradients
% initial guess
x=zeros(n*n,1);
% store norms of residuals
rescg=zeros(niter+1,1);

r = b-A*x;
s = r;
r2 = r'*r;
rescg(1) = sqrt(r2);
for i = 1:niter
  t = A*s;
  s2 = s'*t;
  lambda = r2/s2;
  x = x + lambda*s;
%  fprintf('iteration %3d   x(1)  %20.18f\n',i,x(1))
  r = r-lambda*t;
  r2old = r2;
  r2 = r'*r;
  s=r+(r2/r2old)*s;
  rescg(i+1)=sqrt(r2);
end

%%% Preconditioned conjugate gradients, diagonal preconditioner
% initial guess
x=zeros(n*n,1);
% store norms of residuals
respcgd=zeros(niter+1,1);

M = diag(diag(A));
r = b-A*x;
s = M\r;
r2 = r'*s;
respcgd(1) = sqrt(r2);
for i = 1:niter
  t = A*s;
  s2 = s'*t;
  lambda = r2/s2;
  x = x + lambda*s;
%  fprintf('iteration %3d   x(1)  %20.18f\n',i,x(1))
  r = r-lambda*t;
  r2old = r2;
  z = M\r;
  r2 = r'*z;
  s=z+(r2/r2old)*s;
  respcgd(i+1)=sqrt(r2);
end

%%% Preconditioned conjugate gradients, incomplete Cholesky preconditioner
% initial guess
x=zeros(n*n,1);
% store norms of residuals
respcgic=zeros(niter+1,1);

U=cholinc(A,'0');
x=zeros(n*n,1);
r = b-A*x;
s = U\(U'\r);
r2 = r'*s;
respcgic(1) = sqrt(r2);
for i = 1:niter
  t = A*s;
  s2 = s'*t;
  lambda = r2/s2;
  x = x + lambda*s;
%  fprintf('iteration %3d   x(1)  %20.18f\n',i,x(1))
  r = r-lambda*t;
  r2old = r2;
  z = U\(U'\r);
  r2 = r'*z;
  s=z+(r2/r2old)*s;
  respcgic(i+1)=sqrt(r2);
end

h = 1/(n+1);
x = linspace(0,1,n+2);   % x grid points including boundaries
y = linspace(0,1,n+2);   % y grid points including boundaries

[X,Y] = meshgrid(x,y);      % 2d arrays of x,y values
X = X';                     % transpose so that X(i,j),Y(i,j) are
Y = Y';                     % coordinates of (i,j) grid point, i.e., ((i-1)h,(j-1)h)

Iint = 2:n+1;                 % indices of interior points in x
Jint = 2:n+1;                 % indices of interior points in y
Xint = X(Iint,Jint);        % interior points
Yint = Y(Iint,Jint);

respcgmg=zeros(niter+1,1);
fh = ones(n);          % fh is a mesh function on interior grid points
u = zeros(n);
% compute residual
r = fh;
s = mg5pt(r,u);
r2 = r(:)'*s(:);
respcgmg(1)=sqrt(r2);
for i = 1:niter
  S = [zeros(1,n+2);zeros(n,1),s,zeros(n,1);zeros(1,n+2)];
  t = (4*s-S(1:n,2:n+1)-S(3:n+2,2:n+1)-S(2:n+1,1:n)-S(2:n+1,3:n+2))/h^2;
  s2 = s(:)'*t(:);
  lambda = r2/s2;
  u = u + lambda*s;
  r = r-lambda*t;
  r2old = r2;
  z = mg5pt(r,zeros(n));
  r2 = r(:)'*z(:);
  s=z+(r2/r2old)*s;
  respcgmg(i+1)=sqrt(r2);
end

% plot norms of residuals: linear convergence means a straight line
clf
p=semilogy(ressd,'r');
set(p,'color',[.75,.25,.75],'linewidth',2,'marker','o')
hold on
p=semilogy(rescg);
set(p,'color',[.75,.25,.25],'linewidth',2,'marker','o')
p=semilogy(respcgd);
set(p,'color',[.25,.75,.25],'linewidth',2,'marker','o')
p=semilogy(respcgic,'m');
set(p,'color',[.25,.25,.75],'linewidth',2,'marker','o')
p=semilogy(respcgmg,'r');
set(p,'color',[.75,.75,.25],'linewidth',2,'marker','o')
grid on
set(gca,'fontsize',20)
xlabel('iterations')
ylabel('norm of residual')
legend('SD','CG','CG (diag)','CG (IC)','CG (MG)','Location','SouthWestOutside')

% compute observed rates of convergence by linear fit of log of norm of residuals
lfit=polyfit(1:niter+1,log(ressd)',1);
ratesd = exp(lfit(1));
lfit=polyfit(1:niter+1,log(rescg)',1);
ratecg = exp(lfit(1));
lfit=polyfit(1:niter+1,log(respcgd)',1);
ratepcgd = exp(lfit(1));
lfit=polyfit(1:niter+1,log(respcgic)',1);
ratepcgic = exp(lfit(1));
lfit=polyfit(1:niter+1,log(respcgmg)',1);
ratepcgmg = exp(lfit(1));
fprintf('Observed rates of linear convergence\n')
fprintf('SD:         %6.4f\nCG:         %6.5f\nPCG (diag): %6.4f\nPCG (IC):   %6.4f\nPCG (MG):   %6.4f\n',...
  ratesd,ratecg,ratepcgd,ratepcgic,ratepcgmg)