#include <iostream.h>
#include <iomanip.h>
#include "compact.h"

double product(Vector& u, Vector& v){
	unsigned l = u.length();
	if(l!= v.length()){
		//Warning: different length 
		if(l>v.length()) l = v.length();
	}
	double sum = 0;
	for(int i=1;i<=l;i++)
		sum += (u(i)* v(i));
	return sum;
}

void product(Matrix& A, Vector& u, Vector& result){
	result.clear();
	unsigned n = u.length(), m = A.getcol();
	//u,A.row() must have the same size. User control it! A is compact!
	for(int i=1;i<=n;i++){
		for(int j=i>m?(i-m+1):1;j<=i;j++)
			result(i) += (u(j)*A(i,j+m-i));
		for(int j= (i+m-1<n)?(i+m-1):n;j>i;j--)
			result(i) += (u(j)* A(j,i+m-j));
	}
}

double product(Vector& u, Matrix& A, Vector& v){
	unsigned n = u.length();
	//u,v,A.row() must have the same size. User control it! A is compact!
	Vector tmp(n);
	product(A,u,tmp);
	double sum = product(tmp, v);
	return sum;
}

double CG(Matrix& A, Vector& r, Vector& p, Vector& u){
	double alpha;
	Vector tmp(u.length());
	//alpha(k)
	alpha = product(r,p)/product(p,A,p);
	//u(k)
	tmp = p;
	tmp *= alpha;
	u += tmp;
	//r(k)
	product(A,p,tmp);
	tmp*= alpha;
	r -= tmp;
//	cout<<sqrt(product(r,r))<<endl;
	//p(k)
	tmp = p;
	tmp *= (product(r,A,p)/product(p,A,p));
	p = r;
	p -= tmp;
}

double PCG(Matrix& A, Vector& r, Vector& p, Vector& u){
	double alpha;
	int n = u.length(), m = A.getcol();
	Vector tmp(n);
	//alpha(k)
	alpha = product(r,p)/product(p,A,p);
	//u(k)
	tmp = p;
	tmp *= alpha;
	u += tmp;
	//r(k)
	product(A,p,tmp);
	tmp*= alpha;
	r -= tmp;
//	cout<<sqrt(product(r,r))<<endl;
	//p(k)
	Vector tmp2(n);
	for(int i=1;i<=n;i++)
		tmp2(i) = r(i)/(A(i,m)*A(i,m));
	tmp = p;
	tmp *= (product(tmp2,A,p)/product(p,A,p));
	p = tmp2;
	p -= tmp;
}

int CGsolve(Matrix& B, Vector& b, Vector& u, double ERR=1e-6){
	int k;
	double err;
	//r,p,u
	Vector p(u.length()), r(u.length()), tmp(u.length());
	u.clear();
	r = b;
	p = r;
	//iterate
	k = 0;
	product(B,u,tmp);
	err=error(tmp,p);
	while(err > ERR){
		k+=1;
		tmp = u;
		CG(B,r,p,u);
		err = error(tmp,u);
		cout<<"The "<<k<<"th iteration completed."<<endl;
	}
	return k;
}
int PCGsolve(Matrix& B, Vector& b, Vector& u, double ERR=1e-6){
	int k;
	double err;
	//r,p,u
	Vector p(u.length()), r(u.length()), tmp(u.length());
	u.clear();
	r = b;
	p = r;
	//iterate
	k = 0;
	product(B,u,tmp);
	err=error(tmp,p);
	while(err > ERR){
		k+=1;
		tmp = u;
		PCG(B,r,p,u);
		err = error(tmp,u);
		cout<<"The "<<k<<"th iteration completed."<<endl;
	}
	return k;
}
/*
int main(void){


	int n = 10, m = 2;
	Matrix A(n,n), B(n,m);
	Vector r(n), p(n), u(n), tmp(n);
	//Initialize A
	for(int i=1;i<n;i++){
		A(i,i) = 1.0;
		A(i,i+1) = A(i+1,i) = -0.1;
	}
	A(n,n) = 1e6;
	//B
	symToCompact(A,B);
	//right-hand-side vector
	double data[10] = {0.9,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,999999.9};
	Vector b(data,n);
	int k;
	double err;
	//r,p,u
	u.clear();
	r = b;
	p = r;
	//iterate
	cout<<"Conjugate gradiant method>>>>>>>>>>>>>>>>>>>>>>"<<endl<<endl;
	k = 0;
	product(B,u,tmp);
	err=error(tmp,p);
	cout<<"The initial error is            "<<err<<endl;
	while(err > 1e-7){
		k+=1;
		tmp = u;
		CG(B,r,p,u);
		err = error(tmp,u);
		cout<<"After "<<k<<" iterations the error is "<<err<<endl;
	}
	cout<<endl<<k<<" iterations are needed."<<endl<<endl;

	//Preconditioned
	cout<<"Preconditioned conjugate gradiant method>>>>>>>>"<<endl<<endl;
	u.clear();
	r = b;
	p = r;
	k = 0;
	product(B,u,tmp);
	err=error(tmp,p);
	cout<<"The initial error is            "<<err<<endl;
	while(err > 1e-7){
		k+=1;
		tmp = u;
		PCG(B,r,p,u);
		err = error(tmp,u);
		cout<<"After "<<k<<" iterations the error is "<<err<<endl;
	}
	cout<<endl<<k<<" iterations are needed."<<endl;

	getchar();
}
*/