#include <iostream>
#include <fstream>
#include <map>

#include "TriangleMesh.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 complain
		if (tmp[0] != 'f'
		 || tmp[1] != 'a'
		 || tmp[2] != 'c'
		 || tmp[3] != 'e'
		 || tmp[4] != 't') { file.close(); return(2); }

		//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(2); }
		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::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;
	}
}