// $Id: numRec.cpp 749 2006-06-29 11:44:32Z ninio $

#include "numRec.h"
#include <cassert>

#ifndef VERBOS
#define VERBOS
#endif

void validateSym(VVdouble & v) {
	const MDOUBLE epsilon = 0.00000001;
	for (int i=0; i < v.size(); ++i) {
		for (int j=i+1; j < v.size(); ++j) {
			if (fabs(v[i][j] - v[j][i])> epsilon) {
				LOG(5,<<"v["<<i<<"]["<<j<<"]="<<v[i][j]<<endl);
				LOG(5,<<"v["<<j<<"]["<<i<<"]="<<v[j][i]<<endl);

				errorMsg::reportError("trying to find eigen values to non-sym matrix");
			}
			else v[i][j] = v[j][i];
		}
	}
}

int MyJacobi(VVdouble &Insym, VVdouble &RightEigenV, Vdouble &EigenValues) {
	validateSym(Insym);
	const int MaxNumberOfSweeps = 100000;
	VVdouble& v = RightEigenV;
	VVdouble& a = Insym;
	Vdouble& d = EigenValues;
	//CheckSizeAndTypeAndResizeIfNessary();
	int i,j;
	const int size = v.size();
		
	// preparing V to be the indentity matrix
	for (i=0; i<size; ++i) {
	  for (int j=0; j<size ; ++j) v[i][j]=0.0;
	  v[i][i] = 1.0;
	}
	
		
	for (i=0 ; i<size; ++i ) {
	  d[i] = a[i][i];
	}

	MDOUBLE sm = 0.0; // sm is the sum of the off-diagonal elements
	int ip, iq;
	for (i = 0; i< MaxNumberOfSweeps ; ++i) {
		sm = 0.0;
		for (ip = 0; ip<size ; ++ip) {
			for (iq = ip+1; iq <size; ++iq)	sm +=fabs (a[ip][iq]);
		}
		//if(i%300==0)
		//	LOG(5,<<"sm= "<<sm<<endl);
		if (sm == 0.0) return 0; // the program is suppose to return here, after some rounds of i.
		MDOUBLE tresh;
		if (i<3) tresh = 0.2 * sm / (size*size); else tresh = 0.0;

		MDOUBLE g;
		for (ip=0 ; ip<size; ++ip) {
			for (iq = ip+1 ; iq<size; ++iq) {
				g = 100.0*fabs(a[ip][iq]);

#ifdef VERBOS
	if (g<10e-50) {
		LOG(5,<<"small g!"<<endl);
		if ((i>3 && (fabs(d[ip]+g) == fabs(d[ip])) && (fabs(d[iq]+g)==fabs(d[iq])))==false) {
			LOG(5,<<"g is small: "<<g<< "yes, it is not zeroed"<<endl);
			LOG(5,<<"because d[ip] is: "<<d[ip]<<" and d[iq] is: "<<d[iq]<<endl);
			LOG(5,<<"ip is: "<<ip<<" iq is: "<<iq<<endl);
		}
	}
#endif //VERBOS
				if (i>3 && (fabs(d[ip]+g) == fabs(d[ip])) && (fabs(d[iq]+g)==fabs(d[iq])) ) {
					a[ip][iq] = 0.0;
				}
				else if (fabs(a[ip][iq]) > tresh) {
					MDOUBLE h;
					MDOUBLE t;
					MDOUBLE theta;
					h = d[iq]-d[ip];
	//  assert(h!=0);
					if (fabs(h) + g == fabs(h)) {
						assert(h!=0);
						t = a[ip][iq] / h;
					}
					else {
						theta = 0.5*h/(a[ip][iq]);
						t = 1.0 / (fabs(theta)+sqrt(1.0+theta*theta));
						if (theta<0.0) t = -t;
					}
					MDOUBLE c,s;
					c = 1.0 / sqrt(1.0+t*t);
					s = t*c;
					MDOUBLE tau;
					tau = s/ (1.0 + c);
					h = t * a[ip][iq];
					
					d[ip] = d[ip] - t * a[ip][iq];
					d[iq] = d[iq] + t * a[ip][iq];
					a[ip][iq]=0.0;
					MDOUBLE tmp1, tmp2;
					for (j = 0; j < ip; ++j) {
					  tmp1 = a[j][ip] - s*(a[j][iq]+a[j][ip]*tau); // updating the above element of a...
					  tmp2 = a[j][iq] + s*(a[j][ip]-a[j][iq]*tau);
					  a[j][ip] = tmp1; 
					  a[j][iq] = tmp2;
					}
									
					for (j = ip+1;j<iq; ++j) {
					  tmp1 = a[ip][j] - s*(a[j][iq]+a[ip][j]*tau); // updating the above element of a..
					  tmp2 = a[j][iq] + s*(a[ip][j]-a[j][iq]*tau);
					  a[ip][j] = tmp1;
					  a[j][iq] = tmp2;
					}
					
					for (j = iq+1; j< size ; ++j) {
					  tmp1 = a[ip][j] - s*(a[iq][j]+a[ip][j]*tau); // updating the above element of a..
					  tmp2 = a[iq][j] + s*(a[ip][j]-a[iq][j]*tau);
					  a[ip][j] = tmp1;
					  a[iq][j] = tmp2;
					}
									
					for (j = 0; j< size ; ++j) {
					  tmp1 = v[j][ip] - s*(v[j][iq]+v[j][ip]*tau); // updating v
					  tmp2 = v[j][iq] + s*(v[j][ip]-v[j][iq]*tau);
					  v[j][ip] = tmp1;
					  v[j][iq] = tmp2;
					}
				} // end of "else if (fabs(a[ip][iq] > tresh)"
			} // end of for (iq = ...
		} // end of for (ip = ...
	} // end of for (i = 0; i< MaxNumberOfSweeps ; ++i) {
	vector<string> err;
	err.push_back("problems in function MyJacobi. more than MaxNumberOfSweeps were necesary.");
	errorMsg::reportError(err);
	
	return -1;
} //end of function







///////////////////////////////////////////
//Adi cahnges	//////////////////////////
/////////////////////////////////////////
MDOUBLE sign(MDOUBLE a,MDOUBLE b){ 
	return (b>0?fabs(a):-fabs(a));
}

MDOUBLE pythag(const MDOUBLE a, const MDOUBLE b){
	return sqrt(pow(a,2)+pow(b,2));
}


void houseHolder(VVdouble &mat,VVdouble &Q){
	MDOUBLE sigma=0,H,sqrtSigma,K=0,tmp;
	int c,r,j,i,n = mat.size();
	Q.resize(n);
	for(i=0;i<n;i++){
		Q.resize(n);
	}
	for (i=0;i<n;i++)
		Q[i].resize(n,0.0);
	Vdouble p,q,u;
	p.resize(n,0.0);
	q.resize(n,0.0);
	u.resize(n,0.0);
	for (i=n-1;i>1;i--){
		sigma=0; //init sigma
		K=0;	//init K
	
		for(j=0;j<i;j++)
			sigma+= mat[i][j]*mat[i][j]; //compute sigma: O(n)

		sqrtSigma = mat[i][i-1]>0 ? sqrt(sigma) : -sqrt(sigma); //compute sqrt of sigma +/-
		H=sigma+mat[i][i-1]*sqrtSigma; //comute H = 0.5*|u|^2.  until here O(n)
	
		/***createing U*******/
		for(r=0;r<i;r++) {   //update vector u with row i the matrix until i; //takes  O(n)
			Q[i][r]= u[r] = mat[i][r];
			Q[r][i] = u[r]/H;
		}
		u[i-1]+=sqrtSigma; //update element (i,i-1)
		Q[i][i-1]=u[i-1];
		Q[i-1][i]=u[i-1]/H;
		for(r=i;r<n;r++) //update elemnts (i,j) =0 for j>=i.
			u[r]=0.0;
		/***********************/
		for(r=0;r<n;r++){ //compute vector p O(n^2)
			p[r]=0.0;
			for (c=0;c<i;c++)
				p[r]+=mat[r][c]*u[c]; //compute AU		
			p[r]/=H; // ->AU/H		
		}
	
		for(r=0;r<i;r++) // compure K O(n)
			K+=u[r]*p[r];
		K/=(2*H);
	//	cout<<"K is: "<<K<<endl;
		
		for(r=0;r<n;r++) //compute vector q O(n)
			q[r]=p[r]-K*u[r];
		
		for(r=0;r<=i;r++) {//update matrix O(n^2) only part of the matrix
			for(c=0;c<=i;c++)
				mat[r][c]-=q[r]*u[c]+u[r]*q[c];
		}
					
	}
	for (i=0;i<n;i++){
		for(j=0;j<i;j++){
			tmp=0;
			for(c=0;c<i;c++)
				tmp+=Q[i][c]*Q[c][j];
			for(c=0;c<i;c++)
				Q[c][j]-=tmp*Q[c][i];
		}
		Q[i][i]=1;
		for(j=0;j<i;j++)
			Q[j][i]=Q[i][j]=0.0;
	}
}




void QL(Vdouble &d, Vdouble &e, VVdouble &z){
	int m,l,iter,i,k;
	MDOUBLE s,r,p,g,f,dd,c,b;
		
	int n=d.size();
	//for (i=1;i<n;i++) e[i-1]=e[i];
	//e[n-1]=0.0;
	e.push_back(0);//since in my algorithm I return an n-1 sized e
	for (l=0;l<n;l++) {
		iter=0;
		do {
			for (m=l;m<n-1;m++) {
				dd=fabs(d[m])+fabs(d[m+1]);
				if (fabs(e[m])+dd == dd) break;
			}
			if (m != l) {
				if (iter++ == 30) errorMsg::reportError("Too many iterations in QL");
				g=(d[l+1]-d[l])/(2.0*e[l]);
				r=pythag(g,1.0);
				g=d[m]-d[l]+e[l]/(g+sign(r,g));
				s=c=1.0;
				p=0.0;
				for (i=m-1;i>=l;i--) {
					f=s*e[i];
					b=c*e[i];
					e[i+1]=(r=pythag(f,g));
					if (r == 0.0) {
						d[i+1] -= p;
						e[m]=0.0;
						break;
					}
					s=f/r;
					c=g/r;
					g=d[i+1]-p;
					r=(d[i]-g)*s+2.0*c*b;
					d[i+1]=g+(p=s*r);
					g=c*r-b;
					// Next loop can be omitted if eigenvectors not wanted
					for (k=0;k<n;k++) {
						f=z[k][i+1];
						z[k][i+1]=s*z[k][i]+c*f;
						z[k][i]=c*z[k][i]-s*f;
					}
				}
				if (r == 0.0 && i >= l) continue;
				d[l] -= p;
				e[l]=g;
				e[m]=0.0;
			}
		} while (m != l);
	}
}



/************************************************************************/
//diaganol will be eigen values and fill matrix of eigen vectors.                                                                                   */
/************************************************************************/

void computeEigenSystem(VVdouble &symmetricMatrix,VVdouble &eigenVectros,Vdouble &diagonal){

	houseHolder(symmetricMatrix,eigenVectros);

	Vdouble offdiagonal;
	offdiagonal.resize(symmetricMatrix.size());
	
	for (int i=0; i<symmetricMatrix.size(); i++)
		diagonal[i]=symmetricMatrix[i][i];

	for (int i2=0; i2<symmetricMatrix.size()-1; i2++)
		offdiagonal[i2]=symmetricMatrix[i2+1][i2];


	QL(diagonal,offdiagonal,eigenVectros);
	return;
}
