% Solves Ax=b for x using Conjugate Gradients; A must be symmetric and
% positive-definite
%
% function [x] = solvePD(A,b)
% A - symmetric, positive-definite matrix [n,n]
% b - vector [n,1]
% epsilon - convergence criterion
% imax - maximum number of iterations
%
% Algorithm taken from "An introduction to the conjugate gradient
% method without the agonizing pain" by Jonathan Shewchuk (1994)
% 
% Written by Jason Rennie, November 2003
% Last modified: Tue May 24 17:53:39 2005

function [x] = solvePD(A,b,verbose,epsilon,imax)
fn = mfilename;
if (nargin < 2 | nargin > 5)
  error('Wrong number of arguments (%d)',nargin);
end
if (nargin < 5)
  imax = 1000;
  if (nargin < 4)
    epsilon = 1e-6;
    if (nargin < 3)
      verbose = 1;
    end
  end
end
[n,d] = size(A);
if (n ~= d)
  error('A [%d,%d] must be symmetrc',n,d);
end
[l] = length(b);
if (n ~= l)
  error('width of A [%d,%d] must match length of b [%d,1]',n,d,l);
end

%%%%% Begin Conjugate Gradients algorithm %%%%%
i = 0;
x = zeros(n,1); % [n,1]
r = b - A*x; % [n,1]
d = r; % [n,1]
deltanew = r'*r; % scalar
deltazero = deltanew;
while i < imax & deltanew > epsilon^2*deltazero
  i = i + 1;
  q = A*d; % [n,1]
  alpha = deltanew./(d'*q); % scalar
  xold = x;
  x = x + alpha*d;
  if mod(i,50) == 0
    r = b - A*x;
  else
    r = r - alpha*q;
  end
  deltaold = deltanew;
  deltanew = r'*r;
  beta = deltanew/deltaold;
  d = r + beta*d;
  if (verbose)
    fprintf(1,'%s: i=%d r=%1.0e deltanew=%1.0e xnorm=%1.0e\n',fn,i,sum(abs(r)),deltanew,sum((x-xold).^2));
  end
end