#ifndef __CG_H
#define __CG_H

// Iterative template routine -- CG
// 
// RICHARDSON solves the symmetric positive definite linear
// system Ax=b using the preconditioned conjugate gradient methd.
// The returned value indicates convergence within
// max_iter iterations (return value 0)
// or no convergence within max_iter iterations (return value 1)
// Upon successful return (0), the output arguments have the
// following values:
//        x: computed solution
// mat_iter: number of iterations to satisfy the stopping criterion
//      tol: residual after the final iteration

template <class MATRIX, class VECTOR, class PRECONDITIONER, class REAL>
int
CG (const MATRIX & A, VECTOR & x, const VECTOR & b,
    const PRECONDITIONER & M, int & max_iter, REAL & tol)
{
  REAL resid;
  VECTOR p(b.size ());
  VECTOR z(b.size ());
  VECTOR q(b.size ());
  REAL alpha, beta, rho, rho_1;
  REAL normb = norm (b);
  VECTOR r = b - A * x;
  
  if (normb == 0.0) normb = 1;
  resid = norm (r) / normb;
    
  if (resid <= tol)
    {
      tol = resid;
      max_iter = 0;
      return 0;
    }
  
  for (int i=1; i<=max_iter; i++)
    {
      z = M.solve (r);
      rho = dot (r, z);

      if (i==1)
	{
	  p = z;
	}
      else
	{
	  beta = rho / rho_1;
	  p = z + beta * p;
	}
      
      q = A * p;
      alpha = rho / dot (p, q);
      
      x += alpha * p;
      r -= alpha * q;
      
      resid = norm(r) / normb;
      
      if (resid <= tol)
	{
	  tol = resid;
	  max_iter = i;
	  return 0;
	}
      
      rho_1 = rho;
    }
  
  tol = resid;
  return 1;
}



#endif // __CG_H