double ErrorTolerance = 0.000001;
UINT MaxConvergenceIter = 10000;

void LinearSolver::Init(UINT n)
{
	r.Allocate(n);
	q.Allocate(n);
	p.Allocate(n);
	z.Allocate(n);

	r2.Allocate(n);
	q2.Allocate(n);
	p2.Allocate(n);
	z2.Allocate(n);

	x.Allocate(n);
	b.Allocate(n);
	Temp.Allocate(n);
	DiagTerms.Allocate(n);
	M.Init(n);

	for(UINT i=0;i<n;i++)
	{
		x[i] = 0.0;
	}
}

void LinearSolver::LoadDiagTerms()
{
	int i, N = DiagTerms.Length();
	SparseElement *E;
	for(i=0;i<N;i++)
	{
		if(M.Rows[i].FindElement(i, E))
		{
			DiagTerms[i] = E->Entry;
			if(DiagTerms[i] == 0.0) DiagTerms[i] = 1.0;
		}
		else
			DiagTerms[i] = 1.0;
	}
}

void LinearSolver::CGradSolve()
{
	bool Converged = false;
	double Result, Numerator, Denominator, Alpha, Beta, Error;
	UINT Iter=0;

	//p = d
	Multiply(Temp, M, x);
	Subtract(r, b, Temp);
	for(UINT i=0;i<p.Length();i++)
	{
		p[i] = r[i];
	}
	
	while(!Converged)
	{
		Iter++;

		Numerator = Multiply(r, r);
		Multiply(q, M, p);
		Denominator = Multiply(p, q);

		if(Denominator == 0.0)
		{
			cout << "Zero denominator." << endl;
			return;
		}
		if(Numerator == 0.0)
		{
			cout << "Numerator Zero; no residuals." << endl;
			return;
		}

		Alpha = Numerator / Denominator;

		Multiply(Temp, Alpha, p);
		Add(x, x, Temp);

		if(Iter % 50 == 49)
		{
			Multiply(Temp, M, x);
			Subtract(r, b, Temp);
		} else {
			Multiply(Temp, Alpha, q);
			Subtract(r, r, Temp);
		}

		Result = Multiply(r, r);
		Beta = Result / Numerator;
		
		Multiply(Temp, Beta, p);
		Add(p, r, Temp);

		Numerator = Multiply(r, r);
		Denominator = Multiply(b, b);
		if(Denominator == 0.0)
		{
			cout << "b's zero?" << endl;
			return;
		}
		Error = Numerator / Denominator;
		cout << "Iteration " << Iter << ", Error -> " << Error << endl;
		if(Iter > MaxConvergenceIter || Error < ErrorTolerance)
		{
			Converged = true;
		}
	}
	if(Iter > MaxConvergenceIter) cout << "Convergenced not reached; algorithm halted." << endl;
	else cout << "Convergence, " << Iter << " iterations." << endl;
}

void LinearSolver::BiCGradSolve(double _BMag)
{
	bool Converged = false;
	bool First = true;
	double Beta, Alpha, Row = 0.0, PrevRow, Numerator, Denominator, Error, BMag;
	UINT Iter=0, N = M.Rows.Length();

	LoadDiagTerms();

	BMag = Multiply(b, b);
	if(_BMag != 0.0) BMag = _BMag;
	if(BMag == 0.0)
	{
		cout << "b's zero?" << endl;
		return;
	}

	SparseMatrix M_T;
	M.Transpose(M_T);

	//p = d
	Multiply(Temp, M, x);
	Subtract(r, b, Temp);
	for(UINT i=0;i<N;i++)
	{
		r2[i] = r[i];
	}
	
	while(!Converged)
	{
		Iter++;

		if(Iter % 50 == 49)
		{
			Multiply(Temp, M, x);
			Subtract(r, b, Temp);
			for(UINT i=0;i<N;i++)
			{
				r2[i] = r[i];
			}
			First = true;
		}

		for(UINT i=0;i<N;i++)
		{
			z[i] = r[i] / DiagTerms[i];
			z2[i] = r2[i] / DiagTerms[i];
		}

		PrevRow = Row;
		Row = Multiply(z, r2);
		if(Row == 0.0)
		{
			cout << "Row Zero; Algorithm fails." << endl;
			return;
		}

		if(First)
		{
			for(UINT i=0;i<N;i++)
			{
				p[i] = z[i];
				p2[i] = z2[i];
			}
			First = false;
		} else {
			Beta = Row / PrevRow;
			for(UINT i=0;i<N;i++)
			{
				p[i] = z[i] + Beta * p[i];
				p2[i] = z2[i] + Beta * p2[i];
			}
		}

		Multiply(q, M, p);
		Multiply(q2, M_T, p2);

		Denominator = Multiply(p2, q);
		if(Denominator == 0.0)
		{
			cout << "Denominator zero." << endl;
			return;
		}
		Alpha = Row / Denominator;

		for(UINT i=0;i<N;i++)
		{
			x[i] += Alpha * p[i];
			r[i] -= Alpha * q[i];
			r2[i] -= Alpha * q2[i];
		}

		Numerator = Multiply(r, r);
		Error = Numerator / BMag;
		cout << Iter << "\t" << Error << endl;
		if(Iter > MaxConvergenceIter || Error < ErrorTolerance) Converged = true;
	}
	if(Iter > MaxConvergenceIter)
	{
		cout << "Convergenced not reached; algorithm halted." << endl;
	}
	else
	{
		cout << "Convergence, " << Iter << " iterations." << endl;
	}
}