#include <math.h>
#include "matrix.h"
#include "vector.h"


void printMatrix(Matrix& a){
	int m = a.getrow(), n = a.getcol();
	for(int i=1;i<=m;i++){
		for(int j=1;j<=n;j++)
			cout<<setiosflags(ios::left)<<setw(6)<<a(i,j)<<"  ";
		cout<<endl;
	}
}

void printVector(Vector& v){
	int n = v.length();
	for(int i=1;i<=n;i++)
		cout<<setiosflags(ios::left)<<setw(6)<<v(i)<<"  ";
	cout<<endl;
}

// Compute the error of two vectors in the Euclidian product form
double error(Vector& u, Vector& v){
	int n = u.length();
	double tmp=0;
	if(n>v.length()) n=v.length();// Check the length difference
	
	for(int i=1;i<=n;i++)
		tmp += pow(v(i)-u(i),2);
	return sqrt(tmp);
}


// Translate a symmetrix band matrix "a" with bandwidth "m" 
// into compact mode "b"
void symToCompact(Matrix &a, Matrix &b){
	unsigned n = a.getrow(), m =b.getcol();
    for(int i=1;i<=n;i++)
        for(int j=i<m?m-i+1:1;j<=m;j++){
            b(i,j) = a(i,i+j-m);
		}
}

// Decomposite "b" into "c" using Cholesky decomposition
// Both b and c are in compact mode
void choleskyDecomp(Matrix &b, Matrix &c){
	int i, j, k, n = b.getrow(), m = b.getcol();
	double tmp;
	c(1,m) = sqrt(b(1,m));
	for(i=2;i<=m;i++)
		c(i,m+1-i) = (b(i,m+1-i)/c(1,m));

	for(j=2;j<=(n-1);j++){
		//compute diagonal
		tmp = 0.0;
		for(k=1;k<m;k++){
			tmp += pow(c(j,k),2);
		}
		c(j,m) = sqrt(b(j,m) - tmp);
		//compute c(i,j)
		int bound = (j+m-1)>n?n:(j+m-1);
		for(i=j+1;i<=bound;i++){
			tmp = 0;
			for(k=i-j+1;k<m;k++)
				tmp += (c(i,k-i+j)*c(j,k));
			c(i,j+m-i) = (b(i,j+m-i)-tmp)/c(j,m);
		}
	}
	tmp = 0;
	for(k=1;k<m;k++)
		tmp += pow(c(n,k),2);
	c(n,m) = sqrt(b(n,m) - tmp);
}

// Solve the system Ax=q, 
// where c is the compact mode of Cholesy decomposiotion of A
void solveCholeskySystem(Matrix& c, Vector& q, Vector& x){

	int i, j, n = c.getrow(), m = c.getcol();
	double tmp;
	Vector p(n);
	
	//solve cp=q for p
	for(i=1;i<=n;i++){
		tmp = 0.0;
		for(j=1;j<i;j++)
			tmp += (c(i,j+m-i)*p(j));
		p(i) = (q(i) - tmp)/c(i,m);
	}

	//solve cTx=p
	for(i=n;i>0;i--){
		tmp = 0.0;
		for(j= n<i+m-1?n:i+m-1; j>i;j--)
			tmp += (c(j,i+m-j)*x(j));
		x(i) = (p(i) - tmp)/c(i,m);
	}
}

// SOR method to the system bx=v, with scalar w
// only 1 iteration, update x to be the new value
void SOR(Matrix& b,Vector& v, double w, Vector& x){
	unsigned n = b.getrow(), m = b.getcol();
	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(j,i+m-j)/b(i,m)*x(j));

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

int SORsolve(Matrix& b,Vector& v, double w, Vector& x, double err=1e-6){
	int j;
	Vector y(x.length());
	y.clear();
	for(j=1;;j++){
		y = x;
		SOR(b, v, w, x);
		if(error(x,y)<=err){
			break;
		}
	}
	return j;
}