// *Finding Eigenvalues Of Symmetric Matrix-Jakobi's Method* by Krzysztof Bosak, 1999-01-20

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

#define A a.ValueRaw
#define E e.ValueRaw

void EigenValuesJakobi(const nxnmatrix& a, double precision)
{//Diagonalize
	double t;
	do
	{		
		int j, k;
		double max=0;
		for(int jmax=0; jmax<a.Size()-1; jmax++)
			for(int kmax=jmax+1; kmax<a.Size(); kmax++)
				if(fabs(A(jmax,kmax)) > max)
				{
					j=jmax;
					k=kmax;
					max=fabs(A(jmax,kmax));
				}

		double theta=(A(j,j)-A(k,k))/(2.0*A(j,k));
		if(theta>=0)
			t=1.0/(theta+sqrt(1.0+theta*theta));
		else
			t=-1.0/(-theta+sqrt(1.0+theta*theta));
		double c=1.0/sqrt(1.0+t*t);
		double s=t*c;
		double tau=s/(1.0+c);

			int r=0;
		while(r<j)
		{
			double temp1=A(r,j), temp2=A(r,k);
			A(r,j)+=s*(temp2-tau*temp1);
			A(r,k)-=s*(temp1+tau*temp2);
			r++;
		}
		r++;
		while(r<k)
		{
			double temp1=A(j,r), temp2=A(r,k);
			A(j,r)+=s*(temp2-tau*temp1);
			A(r,k)-=s*(temp1+tau*temp2);
			r++;
		}
		r++;
		while(r<a.Size())
		{
			double temp1=A(j,r), temp2=A(k,r);
			A(j,r)+=s*(temp2-tau*temp1);
			A(k,r)-=s*(temp1+tau*temp2);
			r++;
		}
		A(j,j)+=t*A(j,k);
		A(k,k)-=t*A(j,k);
		A(j,k)=0;
		A(k,j)=0;
	}
	while(fabs(t)>precision);
}

nxnmatrix EigenVectorsJakobi(const nxnmatrix& a, double precision)
{//Diagonalize + find EigenVectors
	//nxnmatrix copy(a);//this line for later checking only
	nxnmatrix e(a.Size());
	for(int i=0; i<a.Size(); i++)
		E(i,i)=1;
	double t;
	do
	{		
		int j, k;
		double max=0;
		for(int jmax=0; jmax<a.Size()-1; jmax++)
			for(int kmax=jmax+1; kmax<a.Size(); kmax++)
				if(fabs(A(jmax,kmax)) > max)
				{
					j=jmax;
					k=kmax;
					max=fabs(A(jmax,kmax));
				}

		double theta=(A(j,j)-A(k,k))/(2.0*A(j,k));
		if(theta>=0)
			t=1.0/(theta+sqrt(1.0+theta*theta));
		else
			t=-1.0/(-theta+sqrt(1.0+theta*theta));
		double c=1.0/sqrt(1.0+t*t);
		double s=t*c;
		double tau=s/(1.0+c);

			int r=0;
		while(r<j)
		{
			double temp1=A(r,j), temp2=A(r,k);
			A(r,j)+=s*(temp2-tau*temp1);
			A(r,k)-=s*(temp1+tau*temp2);
			r++;
		}
		r++;
		while(r<k)
		{
			double temp1=A(j,r), temp2=A(r,k);
			A(j,r)+=s*(temp2-tau*temp1);
			A(r,k)-=s*(temp1+tau*temp2);
			r++;
		}
		r++;
		while(r<a.Size())
		{
			double temp1=A(j,r), temp2=A(k,r);
			A(j,r)+=s*(temp2-tau*temp1);
			A(k,r)-=s*(temp1+tau*temp2);
			r++;
		}
		A(j,j)+=t*A(j,k);
		A(k,k)-=t*A(j,k);
		A(j,k)=0;
		A(k,j)=0;
		#ifndef NDEBUG
		for(int i=0; i<a.Size(); i++)
		{
			double t1=E(j,i), t2=E(k,i);
			E(j,i)=c*t1+s*t2;
			E(k,i)=c*t2-s*t1;
		}
		#endif
	}
	while(fabs(t)>precision);

/* //Nullify very small elements to avoid messy output:
	for(int x=0; x<a.Size(); x++)
		for(int y=0; y<a.Size(); y++)
		{
			if(fabs(A(x,y))<precision)
				A(x,y)=0;
			if(fabs(E(x,y))<precision)
				E(x,y)=0;
		}
*/
/* //Testing the results:
	cout<<endl;
	cout<<"\nEigenVectorsCheck:\n";
	vector v(a.Size()), result(v);
	for(int val=1; val<=e.Size(); val++)
	{
		for(int i=1; i<=e.Size(); i++)
			v.Value(i)=e.Value(val,i);
		
		for(i=1; i<=e.Size(); i++)
			result.Value(i)=0;
		for(i=1; i<=e.Size(); i++)
			for(int j=1; j<=e.Size(); j++)
				result.Value(i)+=copy.Value(j,i)*v.Value(j);
		for(i=1; i<=e.Size(); i++)
			result.Value(i)=result.Value(i)/a.Value(val,val);
		result.Out();
		cout<<endl;
	}*/
	return e;
}

int main()
{
	cout.precision(4);
	const int size=3;
	double data[]={1, 1, 0,  1, 1, 1,  0, 1, 1};
	nxnmatrix m(size, data);
	cout<<"Source Matrix:\n";
	m.Out();
	nxnmatrix eigenvec=EigenVectorsJakobi(m, 1E-6);
	vector eigenval(m.Diagonal());
	
	cout<<"EigenValues:\n";
	eigenval.Out();
	cout<<"EigenVectors by columns:\n";
	eigenvec.Out();
	return 0;
}