#include <cstdio>
#include <cmath>
#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
#include <vector>
#include <iostream>
#include <iomanip>
#include <complex>
#include <Eigen/Dense>

const double PI=4.0*atan(1.0);

using namespace std;

void SetVectorCoefficients(int M,Eigen::VectorXcd& y){ // Make up some stupid vector y
	int i;
	complex<double> ci(0.0,1.0);
	complex<double> c=(1.0+ci)/sqrt(2.0);
	for(i=0;i<M;i++){
		y(i)=1/sqrt(M);
	}
}
void SetMatrixCoefficients(int M,Eigen::MatrixXcd& B){  // Make up some stupid Hermitian matrix B
	int i,j;
	double a=0.6,b=0.26,c=0.3456;
	complex<double> ci(0.0,1.0);
	B.setZero();
	for(i=0;i<M;i++){
		for(j=0;j<M;j++){
			B(i,j)=c+a*double(i+j)+b*ci*double(i-j);
			B(i,j)-=c*sqrt(double(i+j));
		}
	}	
}

int main(int argc,char *argv[]){
	int i,j,M=4;  // dimension M
	Eigen::VectorXcd y(M),yy(M),x(M);                  // the right hand side-vector resulting from the constraints	
	Eigen::MatrixXcd B(M,M),Binv(M,M),C(M,M);
	// Note of vetors and matrices were real we would use MatrixXf and VectorXf
	
	// Fill B and y (values do not mean anything)	
	SetMatrixCoefficients(M,B);
	SetVectorCoefficients(M,y);
	
	cout << "----- Here we solve for x, where y=B*x, and B and y are given\n";
	cout << "_____ y ____\n";
	std::cout << std::fixed << std::setprecision(5) << y << std::endl;
	cout << "_____ B ____\n";
	std::cout << std::fixed << std::setprecision(5) << B << std::endl;
	
	// Solving for x, in equation y=Ax
	x=B.colPivHouseholderQr().solve(y);
	cout << "_____ x:\n";
	std::cout << std::fixed << std::setprecision(5) << x << std::endl;
	// Test that it worked
	yy=B*x;
	cout << "_____ B*x:\n";
	std::cout << std::fixed << std::setprecision(5) << yy << std::endl;
	
	// Test inverse of matrix
	Binv=B.inverse();
	x=Binv*y;
	cout << "---- Binv*y:\n";
	std::cout << std::fixed << std::setprecision(5) << x << std::endl;
	
	C=Binv*B;
	cout << "---- Binv*B:\n";
	std::cout << std::fixed << std::setprecision(5) << C << std::endl;
	
	cout << "\n----- Here we solve for eigenvalues and eigenvectors of B\n";
	
	Eigen::ComplexEigenSolver<Eigen::MatrixXcd> esolver(B);
	Eigen::MatrixXcd Udagger(M,M);    // eigenvectors
	Eigen::VectorXcd lambda(M);  // eigenvalues
	Udagger=esolver.eigenvectors();
	lambda=esolver.eigenvalues();
	cout << "---- Eigenvectors:\n";
	std::cout << std::fixed << std::setprecision(5) << Udagger << std::endl;
	cout << "---- Eigenvalues:\n";
	std::cout << std::fixed << std::setprecision(5) << lambda << std::endl;
	
	Eigen::VectorXcd V(M),W(M);
	for(int i=0;i<M;i++){
		cout << "---------------------\n";
		V=lambda(i)*Udagger.col(i); // make vector V from columns of Udagger
		W=B*Udagger.col(i);
		cout << "---- B*V(" << i << "):\n";
		cout << std::fixed << std::setprecision(5) << W << std::endl;
		cout << "---- lambda*V(" << i << "):\n";
		cout << std::fixed << std::setprecision(5) << V << std::endl;
	}
	Eigen::MatrixXcd U(M,M);
	U=Udagger.conjugate().transpose();  // hermitian conjugate
	C=U*Udagger;
	cout << "---- U*Udagger (U is matrix of eigenvectors):\n";
	cout << std::fixed << std::setprecision(5) << C << std::endl;
	C=U*B*Udagger;
	cout << "---- U*B*Udagger:\n";
	cout << std::fixed << std::setprecision(5) << C << std::endl;
	
	Eigen::MatrixXcd D(M,M);
	D.setIdentity();

	Eigen::VectorXcd v(M),u(M);
	for(i=0;i<M;i++)
		v(i)=1.0+i;
	v=v/sqrt(v.squaredNorm());
	D=B-10.0*D;
	for(i=0;i<150;i++){
		v=D*v;
		v=v/sqrt(v.squaredNorm());
	}
	v=(v/v(0))*sqrt(real(v(0)*conj(v(0))));
	cout << std::fixed << std::setprecision(5) << "---- v:\n" << v << endl;
	
	
	double normU,normv,normtot=0.0;
	complex<double> norm;
	for(i=0;i<M;i++){
		norm=0.0;
		normU=0.0;
		normv=0.0;
		for(j=0;j<M;j++){
			norm+=U(i,j)*v(j);
			normU+=real(U(i,j)*Udagger(j,i));
			normv+=real(v(j)*conj(v(j)));
		}
		normtot+=real(norm*conj(norm));
		
		printf("For eigen-vector %d, overlap with v =%g, normU=%g, normv=%g\n",i,real(norm*conj(norm)),normU,normv);
	}
	printf("normtot=%g\n",normtot);
	
	return 0;
}