#ifndef __CG_H
#define __CG_H
#include <iostream>
#include <fstream>

// 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 = l2norm (b);
  VECTOR r = b - A * x;
  
  VECTOR A1r(b.size ());

  if (normb == 0.0) normb = 1;
  resid = l2norm (r) / normb;
  
  cout << "CG 0: " << resid; cout.flush ();
  //ofstream ofs("cg.gpd");
  //ofs << "0\t" << resid << endl;
  
  if (resid <= tol)
    {
      tol = resid;
      max_iter = 0;
      return 0;
    }
  
  for (int i=1; i<=max_iter; i++)
    {
      M.solve (r, z);
      //z = r;
      rho = inner_product (r, z);

      
      if (i==1)
	{
	  p = z;
	}
      else
	{
	  beta = rho / rho_1;
	  p = z + beta * p;
	}
      
      q = A * p;
      alpha = rho / inner_product (p, q);
      
      x += alpha * p;
      r -= alpha * q;
      
      resid = l2norm(r) / normb;
      
      cout << "\r" << "CG " << i << ": " << resid << "      "; cout.flush ();
      //ofs << i << "\t" << resid << "\t" << rho << endl;
      
      if (resid <= tol)
	{
	  tol = resid;
	  max_iter = i;
	  cout << endl;
	  return 0;
	}
      
      rho_1 = rho;
    }
  
  tol = resid;
  cout << endl;
  //ofs.close ();
  return 1;
}




#endif // __CG_H