% function [x,rnorm] = cg1(x0,b,nits,tol)
% Preconditioned Conjugate Gradient Algorithm:  Implementation 1
% Dianne O'Leary April 1999
%
% Solves the linear system   A x = b  using the conjugate
% gradient algorithm, preconditioned by a matrix  M .
%
% needs functions
%        Ap = Amult(p)  to return the product of A with a vector p
%        Mp = Mmult(p)  to return the product of M with a vector p
%
% input parameters:
%      x0:  initial guess of the true solution
%       b:  right-hand side of the linear system
%    nits:  maximum number of iterations allowed
%     tol:  terminate when ||b-Ax||/||b-Ax0|| < tol
%
% output parameters:
%       x:  approximate solution
%   rnorm:  = ||b-Ax||
%
% This is the standard implementation, due to 
% Hestenes and Stiefel, 1952.



%  bv=[0.0200 0.0100 0.0100 0.0200 0.0100 0 0 0.0100 0.0100 0 0 0.0100 0.0200 0.0100 0.0100 0.0200]';
%This is the vector b st Ax=b when x=1

function [x,rnorm] = cg1(x0,b,nits,tol)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialize the iteration.  Notation:
%   x: current iterate
%   p: search direction
%   r: residual = b - A x
%  Mr: preconditioned residual = M r
%  Ap: = A p
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x = x0;
n = length(x);
Ax = Amult(x);
r=b - Ax;
Mr = Mmult(x,r);
p=Mr;
rMr = r' * Mr;
rnorm0 = norm(r);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Begin the iteration.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=0:nits,

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Update the current iterate and residual.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Ap = Amult(p);
    pAp = p'*Ap;
    alpha = rMr/pAp;
    x = x + alpha * p;
    r = r - alpha *Ap;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check for convergence.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    rnorm=norm(r);
    disp([i,rnorm])
    if (rnorm/rnorm0 < tol)
       disp('convergence attained.')
       break
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the search direction for the next iteration.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    Mr = Mmult(x,r);
    rMr1 = r'*Mr;
    beta = rMr1 / rMr;
    rMr = rMr1;
    p = Mr + beta * p;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End the iteration.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%