#include <iomanip>
#include <cmath>
#include "LinearAlgebra.h"

using namespace std;


namespace numeric {


std::ostream & operator<<( std::ostream & flux, const Vector & v)
{
	for(unsigned i=0;i<v.getDim();++i)
		flux << v(i) << " ";
	return flux;
}


Matrix operator*( const Matrix & a, const Matrix & b)
{
	Matrix result(a);
	result.rightProduct(b);
	return result;
}

Vector operator*( const Matrix & a, const Vector & b)
{
	Vector result(b);
	result.matProduct(a);
	return result;
}

Matrix identity(unsigned n)
{
	Matrix id(n);
	for(unsigned i=0;i<n;i++)
		id(i,i)=1;
	return id;
}

Matrix eigen( const Matrix & m,
						  double * eigenvalues,
							double eps,
							unsigned itermax)
{
 	unsigned iter=0,n=0;
 	Matrix a(m);
 	n=a.getDimRow();
 	Matrix q=identity(n);
 	Matrix saveQ=q;
 	double elmax=1;
	while(elmax > eps && iter < itermax)
	{
		//Initialisation Q<-I
		q= decqr(a);
		saveQ=saveQ*q;
		a=q.transpose()*a*q;

		elmax=0.0;
		for(unsigned j=0;j<n;++j)
		{
			for(unsigned i=j;i<n;++i)
			{
				if( i!=j && elmax < fabs(a(i,j)))
					elmax = fabs(a(i,j));
			}
		}
		iter++;
	}

	if(eigenvalues)
	{
		delete eigenvalues;
	}

	cout << "iter=" << iter << " elmax=" << elmax <<endl;
	cout << fixed << "Q=" << endl << saveQ;
	cout << "R=" << endl << a;
	
	cout << "A=" << endl << saveQ.transpose()*a*saveQ;

	return a;
}

Matrix decqr( Matrix & a)
{
 	unsigned n=a.getDimRow();
 	Matrix q=identity(n);

	for(unsigned k=0;k<n-1;++k)
	{
		Matrix v(n,1);
		double s=0;
		for(unsigned i=k;i<n;++i)
		{
			v(i,0)=a(i,k);
			s+=v(i,0)*v(i,0);
		}

		if( a(k,k) < 0)
			v(k,0)+=sqrt(s);
		else
			v(k,0)-=sqrt(s);

		if( sqrt(s) > 1e-6 )
		{
			double r=0.0f;
	 		for(unsigned i=k;i<n;++i)
					r+=v(i,0)*v(i,0);
			r=sqrt(r);

			for(unsigned i=k;i<n;++i)
					v(i,0)/=r;
		}

 		Matrix h(n);

		for(unsigned j=0;j<n;++j)
		{
			for(unsigned i=0;i<n;++i)
			{
        if(i!=j)
        	h(i,j)=-2*v(i,0)*v(j,0);
				else
        	h(i,j)=1 - 2*v(i,0)*v(j,0);
			}
		}

		q=q*h;
		a=h*a;
	}

	return q;
}

Vector solveUpperLinSys(const Matrix & a, const Vector & c, double eps)
{
	unsigned n = c.getDim();
	Vector x(n);
	double elmax=0.0;
	for(unsigned j=0;j<n;++j)
	{
		for(unsigned i=j;i<n;++i)
		{
			if( i!=j && elmax < fabs(a(i,j)))
				elmax = fabs(a(i,j));
		}
	}

	if(elmax < eps )
	{
		x(n-1) = c(n-1)/a(n-1,n-1);
		for(unsigned j=n-1;j > 0;--j)
		{
			double sum = 0;
			unsigned k = j-1;
			for(unsigned i=j;i<n;++i)
			{
				sum+=x(i)*a(k,i);
			}
			if( abs(a(k,k)) > eps )
				x(k) = (c(k)-sum)/a(k,k);
			else
        x(k) = 0;
		}
	}
	return x;
}

}