#include <cstdlib>
#include <iomanip.h>
#include "CG.h"

#define FUNCTION(x,y) (pi*pi*sin(pi*(x)))
#define BOUNDARY(x,y) 0
#define SOLUTION(x,y) (sin(pi*(x)))

const double pi = 3.141592653589793;

double value(int i, int j, int N){
	double x = double(i)/N, y = double(j)/N;
	return FUNCTION(x,y);
}



void exactSolution(Vector& u,int N){
	u.rescale((N+1)*(N-1));
	for(int i=1;i<N;i++)
		for(int j=1;j<=N+1;j++)
			u((i-1)*(N+1)+j) = SOLUTION(double(i)/N,double(j)/N);
}

// The former error() function computes L2 difference;
// Here we use infinite-norm-difference.
double maxError(Vector& u, Vector& v){
	double err=0;
	int l = u.length();
	for(int i=1;i<=l;i++){
		if(err<fabs(u(i)-v(i))) err = fabs(u(i)-v(i));
	}
	return err;
}

// This SOR is different from former ones
// because here matrix A is not symmetric. So I updated the algorithm.
void SOR2(Matrix& b,Vector& v, double w, Vector& x){
	unsigned n = b.getrow(), m = (b.getcol()+1)/2;
	int i,j;
	double tmp=0;
	for(i=1;i<=n;i++){
		//j<i
		tmp=0;
		for(j= i-m>0?i-m+1:1;j<i;j++)
			tmp -= (b(i,j+m-i)/b(i,m)*x(j));
		//j>i
		for(j=i+m>n?n:i+m-1;j>i;j--)
			tmp -= (b(i,j+m-i)/b(i,m)*x(j));

		tmp += (v(i)/b(i,m));
		//xi (k+1-th)
		x(i) = (1-w)*x(i) + (w * tmp);
	}
}

int SOR2solve(Matrix& b,Vector& v, double w, Vector& x, double err=1e-6){
	int j;
	double error;
	Vector y(x.length());
	y.clear();
	for(j=1;;j++){
		y = x;
		SOR2(b, v, w, x);
		error = maxError(x,y);
		cout<<"\rThe "<<j<<"th iteration completed. The iteration error is "<<error;
		if(error<=err){
			break;
		}
	}
	return j;
}


bool initialize(Matrix& B, Vector& u, Vector& r, int N){
	double h = double(1.0)/N;
	int l = (N-1)*(N+1);
	B.rescale(l,N*2+3);
	u.rescale(l);
	r.rescale(l);
	for(int i=1;i<N;i++){
		for(int j=1;j<=N+1;j++){
			//B
			B((i-1)*(N+1)+j, N+2) = -4;
			if(j!=N+1){
				B((i-1)*(N+1)+j+1, N+1) = 1;
				B((i-1)*(N+1)+j, N+3) = 1;
			}
			if(j==1) B((i-1)*(N+1)+j, N+3) = 2;
			if(j==(N+1)) B((i-1)*(N+1)+j, N+1) = 2;
			if(i>1){
				B((i-1)*(N+1)+j, 1) = 1;
			}
			if(i<N-1){
				B((i-1)*(N+1)+j, N*2+3) = 1;
			}
			//r
			r((i-1)*(N+1)+j) = value(i,j-1,N)*(-1)*h*h;
		}
	}
}

int main(void){
	int N=5;
	double err;
	Matrix  B(1,1),result(1,1); //Always use "B" to denote compact mode of matrix A
	Vector u(1), r(1), exact(1); //u is the solution of scheme, r 
	for(int i=0;i<4;i++){
		initialize(B,u,r,N);
		cout<<"N="<<N<<"       >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"<<endl;
		cout<<"Initialization complete."<<endl;
		cout<<"Matrix B is "<<B.getrow()<<"*"<<B.getcol()<<endl;
		system("PAUSE");
		SOR2solve(B,r,1.0,u);
		exactSolution(exact, N);
		err = maxError(u,exact);
		cout<<endl<<"Truncation error is:"<<err<<endl;
		cout<<"log(error)/log(h) is:"<<(log(err)/log(1.0/N))<<endl;
	    system("PAUSE");
	    cout<<endl;
	    N *=2;
	}	
    return EXIT_SUCCESS;
}