#include <iostream>
#include <stdlib.h>

#include "Vector08.h"
#include "Matrix08.h"

void Jacobi(matrix &A, vector &b, vector &v, 
            unsigned int &count, double tol=1e-6);

//  c=alpha*a + beta*b where a,b are vectors; alpha, beta are scalars
void VecAdd (vector &c, vector &a, vector &b, 
             double alpha=1.0, double beta=1.0);

//  Compute a matrix-vector product (v=A*u)
void MatVec(matrix &A, vector &u, vector &v);


using namespace std;

int main(void )
{
   unsigned int N=4;
   
   matrix A(N);
   vector b(N), c(N);
  
   for (unsigned int i=0; i<N; i++)
   {
      for (unsigned int j=0; j<N; j++)
      {
	 A.setij(i,j, (double) i-j);
	 b.seti(i, 1.0 );
      }
      A.setij(i,i, (double) 8);
   }

   std::cout << "A=" << std::endl;
   A.print();

   c=A*b;
   std::cout << "c=A*b" << std::endl;
   c.print();

   vector est(N);
   unsigned int iterations;

   est.zero();
   Jacobi(A, c, est, iterations, 1e-3);
   std::cout << "Jacobi took " << iterations << " iterations." << std::endl;
   std::cout << "Estimate is: " << std::endl;
   est.print();

   return (0);
}


// Use Jacobi's method to solve Ax=b, 
// On entry : x is the initial guess 
// On entry : x is the estimate for the solution
void Jacobi(matrix &A, vector &b, vector &x, 
            unsigned int &count, double tol)
{
   unsigned int N= A.size();
   count =0;
   if ( (N != b.size()) || (N != x.size() ) )
      std::cout << "Jacobi: error - A must be the same size as b,x\n";
      
   // Form the diagonal and off-diagonal matrices
   matrix Dinv(N), T(N);
   for (unsigned int i=0; i<N; i++)
      for (unsigned int j=0; j<N; j++)
	 if (j != i)
	 {
            T.setij(i,j, -A.getij(i,j));
            Dinv.setij(i,j,0.0);
         }
         else
         {
            T.setij(i,j, 0.0);
            Dinv.setij(i,j, 1.0/A.getij(i,j));
         }      
   
   // Now implement the algorithm:
   vector r(N);
   do
   {
      count++;
      x = Dinv*(b+T*x); // set x = inverse(D)*(b+T*x)
      r=b-A*x; // set r=b-A*r
   }   while ( r.norm() > tol);

   /* std::cout << "Iteration: " << count <<  " Res=" << r.norm() << std::endl; */
   
}