#include <iostream>
#include <fstream>
#include <map>
#include <lapacke.h>

#include "TriangleMesh.h"
#include "../cmath3d_v/TriangleMesh_v.h"

//'multiple' should be ideally 10^desired_decimal_accuracy
int inline RoundTo(const float val, const float multiple=1000.f)
{
	return ( int(floorf(val*multiple)) );
}

//puts v1 into Pos, with mPos being a helper structure preventing having
//v1 multiple times inside the Pos
long unsigned int Enlist(
	const Vector3FC& v1,
	std::vector<Vector3FC>& Pos,
	std::map< int,std::map< int,std::map< int,long unsigned int > > >& mPos)
{
	long unsigned int o1; //ret val

	std::map< int,std::map< int,long unsigned int > >& mY=mPos[RoundTo(v1.x)];
	if (mY.empty())
	{
		Pos.push_back(v1);
		o1=Pos.size();

		//add reference to this vertex in the mPos structure
		std::map< int,long unsigned int > mZ;
		mZ[RoundTo(v1.z)]=o1;
		std::map< int,std::map< int,long unsigned int > > my;
		my[RoundTo(v1.y)]=mZ;
		mPos[RoundTo(v1.x)]=my;
	}
	else
	{
		std::map< int,long unsigned int >& mZ=mY[RoundTo(v1.y)];
		if (mZ.empty())
		{
			Pos.push_back(v1);
			o1=Pos.size();

			//add reference to this vertex in the mPos structure
			std::map< int,long unsigned int > mZ;
			mZ[RoundTo(v1.z)]=o1;
			mY[RoundTo(v1.y)]=mZ;
		}
		else
		{
			if (mZ[RoundTo(v1.z)] == 0)
			{
				Pos.push_back(v1);
				o1=Pos.size();

				//add reference to this vertex in the mPos structure
				mZ[RoundTo(v1.z)]=o1;
			}
			else
			{
				o1=mZ[RoundTo(v1.z)];
			}
		}
	}

	return o1;
}


int ActiveMesh::ImportSTL(const char *filename)
{
	Pos.clear();
	ID.clear();
	norm.clear();

	//a helper map to (efficiently) search for already stored vertices inside Pos
	std::map< int,std::map< int,std::map< int,long unsigned int > > > mPos;
	//         x             y             z  offset+1 in Pos

	//try to open the file
	std::ifstream file(filename);
	if (!file.is_open()) return 1;

	//read the "header" line
	char tmp[1024];
	file >> tmp; //dangerous...
	//check tmp for "solid" or complain
	if (tmp[0] != 's'
	 || tmp[1] != 'o'
	 || tmp[2] != 'l'
	 || tmp[3] != 'i'
	 || tmp[4] != 'd') { file.close(); return(2); }
	//read (and skip) the rest of the header line
	file.ignore(10240,'\n');

	//read facet by facet
	while (file >> tmp)
	{
		//check tmp for "facet" or "endsolid" or complain
		if (tmp[0] != 'f'
		 || tmp[1] != 'a'
		 || tmp[2] != 'c'
		 || tmp[3] != 'e'
		 || tmp[4] != 't')
		{
		//no new face starting, end of file then?
			if (tmp[0] != 'e'
			 || tmp[1] != 'n'
			 || tmp[2] != 'd'
			 || tmp[3] != 's'
			 || tmp[4] != 'o') { file.close(); return(3); }
			else break;
		}

		//read normal
		file >> tmp; //"normal" keyword
		float x,y,z;
		file >> x >> y >> z;
		Vector3F normal(x,y,z);

		//read triangle vertices
		file >> tmp;
		//check tmp for "outer" or complain
		if (tmp[0] != 'o'
		 || tmp[1] != 'u'
		 || tmp[2] != 't'
		 || tmp[3] != 'e'
		 || tmp[4] != 'r') { file.close(); return(4); }
		file >> tmp; //"loop" keyword

		file >> tmp; //"vertex" keyword
		file >> x >> y >> z;
		Vector3FC v1(x,y,z);

		file >> tmp;
		file >> x >> y >> z;
		Vector3FC v2(x,y,z);

		file >> tmp;
		file >> x >> y >> z;
		Vector3FC v3(x,y,z);

		file >> tmp; //"endloop" keyword
		file >> tmp; //"endfacet" keyword

		//add this triangle to the ActiveMesh data structures
		//we need to:
		// scale, round and use this for comparison against already
		// discovered vertices to avoid for having the same vertex saved twice
		long unsigned int o1,o2,o3;
		o1=Enlist(v1,Pos,mPos);
		o2=Enlist(v2,Pos,mPos);
		o3=Enlist(v3,Pos,mPos);
		// 
		// three offsets to the Pos array should be output
		// add them to the ID array
		ID.push_back(o1-1);
		ID.push_back(o2-1);
		ID.push_back(o3-1);
		// add normal to the norm array
		norm.push_back(normal);

		/*
		std::cout << "v1: " << v1.x << "," << v1.y << "," << v1.z << " -- o1=" << o1 << "\n";
		std::cout << "v2: " << v2.x << "," << v2.y << "," << v2.z << " -- o2=" << o2 << "\n";
		std::cout << "v3: " << v3.x << "," << v3.y << "," << v3.z << " -- o3=" << o3 << "\n";
		std::cout << "normal: " << normal.x << "," << normal.y << "," << normal.z << "\n\n";
		*/
	}
	
	file.close();
	return(0);
}


int ActiveMesh::ImportVTK(const char *filename)
{
	Pos.clear();
	ID.clear();
	norm.clear();

	//try to open the file
	std::ifstream file(filename);
	if (!file.is_open()) return 1;




	file.close();
	return(0);
}


void ActiveMesh::RenderMask(i3d::Image3d<i3d::GRAY16>& mask)
{
	//time savers: resolution
	const float xRes=mask.GetResolution().GetX();
	const float yRes=mask.GetResolution().GetY();
	const float zRes=mask.GetResolution().GetZ();

	//time savers: offset
	const float xOff=mask.GetOffset().x;
	const float yOff=mask.GetOffset().y;
	const float zOff=mask.GetOffset().z;

	//over all triangles
	for (unsigned int i=0; i < ID.size()/3; ++i)
	//for (unsigned int i=0; i < 15; ++i)
	{
		const Vector3F& v1=Pos[ID[3*i+0]];
		const Vector3F& v2=Pos[ID[3*i+1]];
		const Vector3F& v3=Pos[ID[3*i+2]];

		//sweeping (and rendering) the triangle
		for (float c=0.f; c <= 1.0f; c += 0.1f)
		for (float b=0.f; b <= (1.0f-c); b += 0.1f)
		{
			float a=1.0f -b -c;

			Vector3F v=a*v1;
			v+=b*v2;
			v+=c*v3;
			
			//std::cout << "ID #" << i << ": v=(" << v.x << "," << v.y << "," << v.z << ")\n";

			//nearest neighbor pixel coordinate
			const int x=(int)roundf( (v.x-xOff) *xRes);
			const int y=(int)roundf( (v.y-yOff) *yRes);
			const int z=(int)roundf( (v.z-zOff) *zRes);

			if (mask.Include(x,y,z)) mask.SetVoxel(x,y,z,short(i));
		}
	}
}


void ActiveMesh::RenderMaskB(i3d::Image3d<i3d::GRAY16>& mask)
{
	//time savers: resolution
	const float xRes=mask.GetResolution().GetX();
	const float yRes=mask.GetResolution().GetY();
	const float zRes=mask.GetResolution().GetZ();

	//time savers: offset
	const float xOff=mask.GetOffset().x;
	const float yOff=mask.GetOffset().y;
	const float zOff=mask.GetOffset().z;

	//over all triangles
	//for (unsigned int i=0; i < ID.size()/3; ++i)
	for (unsigned int i=0; i < 15; ++i)
	{
		const Vector3F& v1=Pos[ID[3*i+0]];
		const Vector3F& v2=Pos[ID[3*i+1]];
		const Vector3F& v3=Pos[ID[3*i+2]];

		//Vector3F e12=v2-v1; //ID=0
		//Vector3F e13=v3-v1; //ID=1
		//Vector3F e23=v3-v2; //ID=2
		Vector3F edges[3]={v2-v1,v3-v1,v3-v2};

		short order[3]={0,1,2};
		if (edges[1].LenQ() < edges[0].LenQ()) { order[0]=1; order[1]=0; }
		if (edges[2].LenQ() < edges[order[1]].LenQ())
		{
			order[2]=order[1];
			order[1]=2;

			if (edges[2].LenQ() < edges[order[0]].LenQ())
			{
				order[1]=order[0];
				order[0]=2;
			}
		}

		//point within a triangle will of the form: vv + b*vb + c*vc
		//vb is the shortest edge, vc is the longest edge
		//vv is their common vertex
		//vb,vc are oriented outward from this vertex
		Vector3F vv,vb,vc;
		if (order[0]==0 && order[2]==1)
		{
			//order[0] "points" at shortest edge
			vv=v1;
			vb=edges[0];
			vc=edges[1];
		}
		else
		if (order[0]==0 && order[2]==2)
		{
			vv=v2;
			vb=-edges[0];
			vc=edges[2];
		}
		else
		if (order[0]==1 && order[2]==0)
		{
			vv=v1;
			vb=edges[1];
			vc=edges[0];
		}
		else
		if (order[0]==1 && order[2]==2)
		{
			vv=v3;
			vb=-edges[1];
			vc=-edges[2];
		}
		else
		if (order[0]==2 && order[2]==0)
		{
			vv=v2;
			vb=edges[2];
			vc=-edges[0];
		}
		else
		if (order[0]==2 && order[2]==1)
		{
			vv=v3;
			vb=-edges[2];
			vc=-edges[1];
		}

		//optimal increment for the short and long edge, respectively
		float db=(vb.x*xRes > vb.y*yRes)? vb.x*xRes : vb.y*yRes;
		db=(vb.z*zRes > db)? vb.z*zRes : db;
		db=1.f/db;

		float dc=(vc.x*xRes > vc.y*yRes)? vc.x*xRes : vc.y*yRes;
		dc=(vc.z*zRes > dc)? vc.z*zRes : dc;
		dc=1.f/dc;

		std::cout << "\nID #" << i << ": v1=(" << v1.x << "," << v1.y << "," << v1.z << ")\n";
		std::cout << "ID #" << i << ": v2=(" << v2.x << "," << v2.y << "," << v2.z << ")\n";
		std::cout << "ID #" << i << ": v3=(" << v3.x << "," << v3.y << "," << v3.z << ")\n";
		
		//sweeping (and rendering) the triangle
		for (float c=0.f; c <= 1.0f; c += dc)
		for (float b=0.f; b <= (1.0f-c); b += db)
		{
			Vector3F v=vv;
			v+=b*vb;
			v+=c*vc;
			
			std::cout << "ID #" << i << ": v=(" << v.x << "," << v.y << "," << v.z << ")\n";

			//nearest neighbor pixel coordinate
			const int x=(int)roundf( (v.x-xOff) *xRes);
			const int y=(int)roundf( (v.y-yOff) *yRes);
			const int z=(int)roundf( (v.z-zOff) *zRes);

			if (mask.Include(x,y,z)) mask.SetVoxel(x,y,z,short(i));
		}
	}
}


void ActiveMesh::CenterMesh(const Vector3F& newCentre)
{
	//calc geom. centre
	double x=0.,y=0.,z=0.;
	for (unsigned int i=0; i < Pos.size(); ++i)
	{
		x+=Pos[i].x;
		y+=Pos[i].y;
		z+=Pos[i].z;
	}
	x/=double(Pos.size());
	y/=double(Pos.size());
	z/=double(Pos.size());

	x-=newCentre.x;
	y-=newCentre.y;
	z-=newCentre.z;

	//shift the centre to point (0,0,0)
	for (unsigned int i=0; i < Pos.size(); ++i)
	{
		Pos[i].x-=float(x);
		Pos[i].y-=float(y);
		Pos[i].z-=float(z);
	}
}


void ActiveMesh::ScaleMesh(const Vector3F& scale)
{
	for (unsigned int i=0; i < Pos.size(); ++i)
	{
		Pos[i].x*=scale.x;
		Pos[i].y*=scale.y;
		Pos[i].z*=scale.z;
	}
}


// ============== surface fitting ============== 
int ActiveMesh::CalcQuadricSurface_Taubin(const int vertexID,
                                          float (&coeffs)[10])
{
	//determine some reasonable number of nearest neighbors
	std::vector< std::vector<size_t> > neigsLeveled;
	ulm::getVertexNeighbours(*this,vertexID,2,neigsLeveled);

	//make it flat...
	std::vector<size_t> neigs;
	for (unsigned int l=0; l < neigsLeveled.size(); ++l)
		for (unsigned int i=0; i < neigsLeveled[l].size(); ++i)
			neigs.push_back(neigsLeveled[l][i]);
	neigsLeveled.clear();

	//V be the vector of [x,y,z] combinations, a counterpart to coeffs
	float V[10],V1[10],V2[10],V3[10];

	//M be the matrix holding initially sum of (square matrices) V*V'
	//N is similar, just a sum of three different Vs is used
	float M[100],N[100];
	for (int i=0; i < 100; ++i) M[i]=N[i]=0.f;

	//over all neighbors (including the centre vertex itself),
	//
	//this order garuantees that array V will be relevant for input
	//vertexID after the cycles are over -- will become handy later
	for (signed int n=(int)neigs.size()-1; n >= 0; --n)
	{
		//shortcut to the current point
		const Vector3FC& v=Pos[neigs[n]];

		//get Vs for the given point 'v'
		V[0]=1.f;       V1[0]=0.f;      V2[0]=0.f;      V3[0]=0.f;
		V[1]=v.x;       V1[1]=1.f;      V2[1]=0.f;      V3[1]=0.f;
		V[2]=v.y;       V1[2]=0.f;      V2[2]=1.f;      V3[2]=0.f;
		V[3]=v.z;       V1[3]=0.f;      V2[3]=0.f;      V3[3]=1.f;
		V[4]=v.x*v.y;   V1[4]=v.y;      V2[4]=v.x;      V3[4]=0.f;
		V[5]=v.x*v.z;   V1[5]=v.z;      V2[5]=0.f;      V3[5]=v.x;
		V[6]=v.y*v.z;   V1[6]=0.f;      V2[6]=v.z;      V3[6]=v.y;
		V[7]=v.x*v.x;   V1[7]=2.f*v.x;  V2[7]=0.f;      V3[7]=0.f;
		V[8]=v.y*v.y;   V1[8]=0.f;      V2[8]=2.f*v.y;  V3[8]=0.f;
		V[9]=v.z*v.z;   V1[9]=0.f;      V2[9]=0.f;      V3[9]=2.f*v.z;

		//construct V*V' and add it to M and N
		for (int j=0; j < 9; ++j)  //for column
		 for (int i=0; i < 9; ++i) //for row;  note the order optimal for Fortran
		 {
			//C order                 (row-major):  M_i,j -> M[i][j] -> &M +i*STRIDE +j
			//Lapack/Fortran order (column-major):  M_i,j -> M[i][j] -> &M +j*STRIDE +i
			//const int off=i*10 +j; //C
			const int off=j*9 +i; //Fortran

			M[off]+= V[j+1]* V[i+1];
			N[off]+=V1[j+1]*V1[i+1];
			N[off]+=V2[j+1]*V2[i+1];
			N[off]+=V3[j+1]*V3[i+1];
		 }
	}

	//now, solve the generalized eigenvector of the matrix pair (M,N):
	//MC = nNC
	//
	//M,N are (reduced) 9x9 matrices constructed above,
	//C is vector (infact, the coeff), n is scalar Lagrange multiplier
	//
	//if M,N were (full-size) 10x10 matrices, the N would be singular
	//as the 1st row would contain only zeros,
	//it is therefore reduced to 9x9 sacrifing the first row
	//
	//according to netlib (Lapack) docs, http://www.netlib.org/lapack/lug/node34.html
	//type 1, Az=lBz -- A=M, B=N, z=C
	//function: SSYGV
	lapack_int itype=1;
	char jobz='V';
	char uplo='U';
	lapack_int n=9;
	float w[10];
	float work[512];
	lapack_int lwork=512;
	lapack_int info;
	LAPACK_ssygv(&itype,&jobz,&uplo,&n,M,&n,N,&n,w,work,&lwork,&info);

	std::cout << "vertices considered: " << neigs.size() << "\n";
	std::cout << "info=" << info << " (0 is OK)\n";
	std::cout << "work(1)=" << work[0] << " (should be below 512)\n";
	
	//if some error, report it to the caller
	if (info != 0) return info;

	//M is now matrix of eigenvectors
	//it should hold (according to Lapack docs):
	//Z^T N Z = I where Z is one eigenvector, I is identity matrix
	//
	//w holds eigenvalues in ascending order

	//our result c[1]...c[9] is the eigenvector
	//corresponding to the smallest non-negative eigenvalue, so the j-th eigenvector
	int j=0;
	while (j < 9 && w[j] < 0.f) ++j;
	//have we found some non-negative eigenvalue?
	if (j == 9) return(-9999);

	//also:
	//the last missing coefficient c[0] we will determine by submitting
	//the given input vertex to the algebraic expresion of the surface
	//(given with coeffs) and equating it to zero:
	coeffs[0]=0.f;
	for (int i=0; i < 9; ++i)
	{
		coeffs[i+1]=M[j*9 +i];          //copy eigenvector
		coeffs[0]-=coeffs[i+1]*V[i+1];  //determine c[0]
	}

	std::cout << "w(j)=" << w[j] << ", j=" << j << "\n";

	return 0;
}