// RunGaussSeidel.cpp for CS319 Lab 6 
//   What: Solution for implementation if G-S
//     Uses that we have overloaded (vector)/(matrix) for
//    an lower triangular matrix, L 
// Author: Niall Madden
//   Date: March 2017

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

#include "Vector08.h"
#include "Matrix08soln.h" 

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

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

int main(void )
{
   // Our matrix is of size NxN
   unsigned int N=6;
   
   matrix A(N);
   vector x(N), b(N); // A*x=b
  
   // Initialise the entries of A. We choose the off-diagonal entries
   // to be random numbers between -3 and 3. The  diagonal entries, 
   // will be the sum of the absolute value of the others +1.
   // We make the random entries in [-3,3] by picking a random number
   // between 0 and 6, and then subtracting 3.
   // The entries in the solution vector, x,  are all 1.
   // Then we choose the solution right-hand side so A*x=b
   for (unsigned int i=0; i<N; i++)
   {
      A.setij(i,i, 0.0);
      for (unsigned int j=0; j<N; j++)
	 if (i != j)
	 {
	    A.setij(i,j, (double) (rand()%6-3));
	    A.setij(i,i, A.getij(i,i) + abs(A.getij(i,j)));
	 }
      x.seti(i, 1.0 );
      A.setij(i,i, A.getij(i,i)+1.0);
   }

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

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

   // We will now use the Jacobi method to approximate the solution
   // The Jacobi solution is called estimate
   vector estimate(N);
   unsigned int iterations;

   estimate.zero();
   // Call Jacobi's method. Want to have the norm of the residual less
   //  than 10^(-4)
   Jacobi(A, b, estimate, iterations, 1e-4);
   std::cout << "Jacobi took " << iterations << " iterations. Estimate is : "
	     << std::endl;
   estimate.print();
   std::cout << "The norm of the error is: ";
   vector error=x-estimate;
   std::cout << error.norm() << std::endl << std::endl;

   // Now call Gauss-Seidel
   estimate.zero();
   GaussSeidel(A, b, estimate, iterations, 1e-3);
   std::cout << "Gauss-Seidel took " << iterations << " iterations." <<std::endl;
   std::cout << "The estimate is : " << std::endl;
   estimate.print();
   error=x-estimate;
   std::cout << "The error is : " << error.norm() << std::endl << std::endl;;

   // Not sure about the next bit: might need "#ifdef WIN32" or "#ifdef WIN64"
   #ifdef _WIN32
   system("PAUSE");
   #endif
   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; */
   
}

// Use the GaussSeidel method to solve Ax=b, 
// On entry : x is the initial guess 
// On entry : x is the estimate for the solution
void GaussSeidel(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 L(N), U(N);
   for (unsigned int i=0; i<N; i++)
      for (unsigned int j=0; j<N; j++)
	 if (i>=j)
	 {
	    L.setij(i,j, A.getij(i,j));
	    U.setij(i,j, 0);
	 }
         else
	 {
            U.setij(i,j, -A.getij(i,j));
            L.setij(i,j, 0);
	 }   
   // Now implement the algorithm:
   vector r(N);
   do
   {
      count++;
      x = (U*x+b)/L; // 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; */
   
}