// *Jacobi's Iteration, solves Ax=b* by Krzysztof Bosak 1999-01-10

#include <iostream.h>
#include <math.h>
#include <memory.h>
#define MAIN
#include "../joint/l3_mat.cpp"

bool SwapForDiagNZ(nxnmatrix &m, vector &b)
{
	int size=m.Size();
	bool *lock=new bool[size+1];
	int *zero=new int[size+1];
	memset(lock, 0, size+1);
	memset(zero, 0, (size+1)*sizeof(int));
	int x;
	for(x=1; x<=size; x++)
	{
		for(int y=1; y<=size; y++)
			if(m.Value(x, y)==0)
				zero[x]++;
	}	
	for(x=1; x<=size; x++)
		if(zero[x]>=size || zero[x]<0)
		{
			delete[] zero;
			delete[] lock;
			return true;
		}
	int maxzero;
	do
	{
		maxzero=0;
		int maxx=1;
		for(int x=1; x<=size; x++)
			if(!lock[x] && zero[x]>maxzero)
			{
				maxzero=zero[x];
				maxx=x;
			}
		double maxabs=0;
		int maxy;
		for(int y=1; y<=size; y++)
			if(fabs(m.Value(maxx, y))>maxabs)
			{
				maxabs=fabs(m.Value(maxx, y));
				maxy=y;
			}
		m.SwapRow(maxx, maxy);
		b.Swap(maxx, maxy);
		lock[maxx]=true;
	}while(maxzero);
	delete[] zero;
	delete[] lock;
	return false;
}

const vector Jacobi(const nxnmatrix &m, const vector &bvect, const double precision)
{
	nxnmatrix mul(-m);
	int size=m.Size();
	vector diag(size);
	for(int i=1; i<=mul.Size(); i++)
	{
		diag.Value(i)=-mul.Value(i, i);
		mul.Value(i, i)=0;
	}
	vector vect(size), diff(size);
	bool next;
	int miniter=5;
	do
	{
		vect=mul*vect;
		vect+=bvect;
		for(int i=1; i<=size; i++)
			vect.Value(i)/=diag.Value(i);
		next=false;
		diff=m*vect;
		diff-=bvect;
//		diff.Out();
//		cout<<endl;
		for(int test=1; test<=size; test++)
		    if(fabs(diff.Value(test))>precision)
		    {
				next=true;
				continue;
		    }
	}while(next || --miniter);
	return vect;
}

int main()
{
	int size=3;
	double adat[]={0, 1, 2, 2, 5, 2, 1, 0, 9, 1, 1, 1, 1, 1, 3, 1};
	double bdat[]={1, 2, 3, 4};

	nxnmatrix A(size, adat);
	vector b(size, bdat);
	SwapForDiagNZ(A, b);
	vector result(Jacobi(A, b, 1E-6));
	cout<<"----- A: -----\n";
	A.Out();
	cout<<"----- x: -----\n";
	result.Out();
	cout<<"----- b: -----\n";
	b.Out();
	cout<<"----- r=Ax-b: -----\n";
	result=A*result;
	result-=b;
	result.Out();
	cout<<endl;
	return 0;
}