#include <iostream>
#include <iomanip>
#include <math.h>

typedef void (*MatrixAction) (double*, double*, int n);

double SCALE = 1;

/**************************** Some Usefull One-Liners *******************/
// y = alpha*x (x,y - vectors, alpha - scalar)
inline void vector_copy(double *y, double *x, int n, double alpha = 1.0)
{
   for(int i=0; i<n; i++)
      y[i] = alpha*x[i];
}

// y = y + alpha*x (x,y - vectors, alpha - scalar)
inline void vector_add(double *y, double *x, int n, double alpha = 1.0)
{
   for(int i=0; i<n; i++)
      y[i] += alpha*x[i];
}

inline double vector_inner_product(double *x, double *y, int n)
{
   double result=0;
   for(int i=0; i<n; i++)
      result += x[i]*y[i];
   return result;
}

/************************** Helper procedures (inline) ************************/
inline void OUTPUT(int iter, double* u, int n)
{
   std::cout << std::setw(4) << iter << " : (";
   for (int i=0; i<n-1; i++)
      std::cout << std::setw(10) << u[i] << ", ";
   std::cout << std::setw(10) << u[n-1];
   std::cout << ")" << std::endl;
}


inline void EXTRAP(double gamma, int n, MatrixAction G, double *c, double *u, double *v)
{
   //Line 1 of Extrap
   vector_copy(v,c,n,gamma); // v = gamma*c
   vector_add(v,u,n,(1.0-gamma));

   //Allocating memory at each step is especially bad (and time consuming)!
   //I've done it only to preserve the pseudo-code from your book
   double *temp = new double[n];

   //Line 2 of Extrap
   G(temp,u,n);
   vector_add(v, temp,n,gamma);
   
   delete temp;
}

inline void CHEB(double rho, double gamma, int n, MatrixAction G, double *c, double *u, double *v)
{
   //Line 1 of Cheb
   vector_copy(u,u,n,(1.0-rho));
   vector_add(u,v,n,rho*(1.0-gamma));
   vector_add(u,c,n,rho*gamma);

   //Allocating memory at each step is especially bad (and time consuming)!
   //I've done it only to preserve the pseudo-code from your book
   double *temp = new double[n];

   //Line 2 of Cheb
   G(temp,v,n);
   vector_add(u,temp,n,rho*gamma);
   
   delete temp;
}

/*********************** The Main Attraction *************************/

// A procedure implementing the Chebyshev Acceleration algorithm for
// the iteration method x^(k+1)=Gx^k+c, where the parameters are:
// G   - a function implementing the action of the iteration matrix,
// c   - a vector containing the vector c in the iteration equation,
// x   - a vector containing the initial guess at the begining and
//       the final approximation at the end,
// n   - the dimention of the vectors x,c (and the sqare matrix G)
// a   - lower bound for the eigenvalues of G 
// b   - upper bound for the eigenvalues of G (must be less than 1)
// eps      = stopping criteria (not implemented)
// max_iter = the maximum number of iterations allowed

int solveCheb(MatrixAction G, double *c, double *x, int n, double a, double b,
            double eps = 0.1e-20, int max_iter = 10)
{
   //We can do without some of these variables, if we reuse others
   //  I have not tried to optimize the code, but to write it as
   //  clear as possible.
   
   double *u = new double[n];
   double *v = new double[n];
   int iter=0;
   vector_copy(u,x,n);  //our initial guess was x;

   //1
   //2
   double gamma = 2.0/(2 - b - a);
   //3
   double alpha = pow((0.5*(b-a)/(2-b-a)),2);
   //4
   OUTPUT(0,u,n);
   //5
   EXTRAP(gamma, n, G, c, u, v);
   //6
   OUTPUT(1,v,n);   
   //7
   double rho = 1.0/(1.0-2*alpha);
   //8
   CHEB(rho, gamma,n, G, c, u, v);
   //9
   OUTPUT(2,u,n);   
   //10
   for(iter=3; iter<max_iter; iter+=2) {
      //11
      rho = 1.0/(1.0 - rho*alpha); //!!
      //12
      CHEB(rho, gamma,n, G, c, v, u);
      //13
      OUTPUT(iter,v,n);      
      //14
      rho = 1.0/(1.0 - rho*alpha);
      //15
      CHEB(rho, gamma,n, G, c, u, v);
      //16
      OUTPUT(iter+1,v,n);      
      //17
      /*   if (max_diff(u,v,n) < eps) break;  */
      //18
   }

   vector_copy(x,u,n);

   delete u;
   delete v;
   return iter;
}


/********************* Data For the Particular Problem ***********************/
   
// This is the action of the Jacobi iteration matrix G for our problem.
// See your textbooks pg.243 fir details...
void myMatrix(double* out, double* in, int n)
{
   out[0] = (1-SCALE*4)*in[0]+SCALE*(in[1]+in[2]);
   out[1] = (1-SCALE*4)*in[1]+SCALE*(in[0]+in[3]);
   out[2] = (1-SCALE*4)*in[2]+SCALE*(in[0]+in[3]);
   out[3] = (1-SCALE*4)*in[3]+SCALE*(in[1]+in[2]);
}


int main()
{
   int n=4;
   int iter=10;

   std::cout << "Enter the scaling of the system: ";
   std::cin >> SCALE;
   

   int CHEB = 0;
   std::cout << "Enter 1 for Chebyshev Acceleration or 0"
      "for the standard Richardson iteration: ";
   std::cin >> CHEB;
   
   if (CHEB) {
      double *c = new double[n];
      c[0]=-SCALE*4; c[1]=0; c[2]=SCALE*4; c[3]=-SCALE*4;

      double *x = new double[n];
      x[0]=0; x[1]=0; x[2]=0; x[3]=0;

      std::cout << "******* Chebyshev Acceleration "
         " of the Richardson Method ******" << std::endl;
      iter = solveCheb(myMatrix, c, x, n, 1-6*SCALE, 1-2*SCALE);

      delete c;
      delete x;
   }
   else {
      double *c = new double[n];
      c[0]=-SCALE*4; c[1]=0; c[2]=SCALE*4; c[3]=-SCALE*4;

      double *x = new double[n];
      x[0]=0; x[1]=0; x[2]=0; x[3]=0;

      double *temp = new double[n];

      std::cout << "******* The Standard Version of "
         " the Richardson Method ******" << std::endl;

      std::cout << "Enter the number of iterations: ";
      std::cin >> iter;

      
      for (int i=0; i<iter; i++)
      {
         myMatrix(temp,x, n);
         vector_copy(x,temp,n);
         vector_add(x,c,n);
         OUTPUT(i,x,n);
      }

      delete c;
      delete x;
   }
}