#include "BaseGL.h"
#include "Tetobject.h"

Tetobject::Tetobject()
{
	nofVertices = 0;
	nofTetras = 0;
	driver = new Driver();
}

bool Tetobject::LoadTetraFile(char *filename, float stiffness, float mass) {
	//static float stiffness = 5;
	int i;
	int nofMaterials = 0;
	int nofTriangles = 0;
	nofVertices = 0;
	nofTetras = 0;
	// Open file
	FILE *f = fopen(filename, "r");
	if (!f) return false;
	const int STRING_LEN(256);
	char s[STRING_LEN];

	// Check version
	fgets(s, STRING_LEN, f);		// get STRING_LEN chars, max. one line
	if (strncmp(s, "tet version 1.0",15)) {
		fclose(f);
		return false;
	}
	fgets(s, STRING_LEN, f);		// get Number of Materials
	if (sscanf(s, "num_materials %i", &nofMaterials) != 1) {
		fclose(f);
		return false;
	}
	fgets(s, STRING_LEN, f);		// get Number of Vertices
	if (sscanf(s, "num_vertices %i", &nofVertices) != 1) {
		fclose(f);
		return false;
	}
	fgets(s, STRING_LEN, f);		// get Number of Tetras
	if (sscanf(s, "num_tetras %i", &nofTetras) != 1) {
		fclose(f);
		return false;
	}
	fgets(s, STRING_LEN, f);		// get Number of Triangles
	if (sscanf(s, "num_triangles %i", &nofTriangles) != 1) {
		fclose(f);
		return false;
	}
	fgets(s, STRING_LEN, f);		// next line
	while (strncmp(s, "MATERIALS", 9))	// search for line that begins with "..."
		fgets(s, STRING_LEN, f);
	
	// Allocate Arrays
	this->nofVertices = nofVertices;
	this->nofTetras = nofTetras;
	vertexArray = (Vertex**) malloc(sizeof(Vertex*) * nofVertices);
	tetraArray = (Tetra**) malloc(sizeof(Tetra*) * nofTetras);

	// *** MATERIALS ***
	fgets(s, STRING_LEN, f);
	while (strncmp(s, "VERTICES", 8)) {
		fgets(s, STRING_LEN, f);
	}
	
	// *** VERTICES ***
	i = 0;
	fgets(s, STRING_LEN, f);
	while (strncmp(s, "TETRAS", 6)) {	// get all VERTICES
		SbVec3f v;
		sscanf(s, "%f %f %f", &v[0], &v[1], &v[2]);
		Vertex *vertex = new Vertex();	// make Vertex
		vertex->pos = v;				// store position of Vertex
		vertex->mass = mass;			// store standard mass
		if (i >= nofVertices) {			// out of boundary check
			fclose(f);
			printf("Error: more vertices than in file defined.\n");
			return false;
		}
		vertexArray[i++] = vertex;		// add Vertex to array
		fgets(s, STRING_LEN, f);
	}

	// *** TETRAS ***
	i = 0;
	fgets(s, STRING_LEN, f);
	while (!feof(f) && strncmp(s, "TRIANGLES", 9)) {	// get all TETRAS
		int i1, i2, i3, i4, matNr;
		sscanf(s, "%i %i %i %i %i", &i1, &i2, &i3, &i4, &matNr);
		
		Tetra *tetra = new Tetra();	// make Tetra
		tetra->p1 = vertexArray[i1];
		tetra->n1 = i1;
		tetra->p2 = vertexArray[i2];
		tetra->n2 = i2;
		tetra->p3 = vertexArray[i3];
		tetra->n3 = i3;
		tetra->p4 = vertexArray[i4];
		tetra->n4 = i4;
		if (i >= nofTetras) {			// out of boundary check
			fclose(f);
			printf("Error: more tetras than in file defined.\n");
			return false;
		}
		tetraArray[i++] = tetra;	// add Tetra to array
		fgets(s, STRING_LEN, f);
	}
	nofTetras = i;		// Number of tetras

	// Create Springs
	bool** adjacent;		// Create adjacentMatrix dynamically, from each to each vertex
	adjacent = new bool*[nofVertices];
	for (i=0; i<nofVertices; i++)
		adjacent[i] = new bool[nofVertices];

	int j;
	for (j=0; j<nofVertices; j++) {		// init adjacentMatrix
		for (i=0; i<nofVertices; i++) {
			adjacent[i][j] = false;
		}
	}
	nofSprings = 0;
	for (i=0; i<nofTetras; i++) {	// for each tetra
		// create 6 springs if not already exist
		if (false == adjacent[tetraArray[i]->n1][tetraArray[i]->n2]) {
			springArray[nofSprings++] = new Spring(vertexArray[tetraArray[i]->n1], vertexArray[tetraArray[i]->n2], stiffness);
			adjacent[tetraArray[i]->n1][tetraArray[i]->n2] = true;
			adjacent[tetraArray[i]->n2][tetraArray[i]->n1] = true;
		}
		if (false == adjacent[tetraArray[i]->n1][tetraArray[i]->n3]) {
			springArray[nofSprings++] = new Spring(vertexArray[tetraArray[i]->n1], vertexArray[tetraArray[i]->n3], stiffness);
			adjacent[tetraArray[i]->n1][tetraArray[i]->n3] = true;
			adjacent[tetraArray[i]->n3][tetraArray[i]->n1] = true;
		}
		if (false == adjacent[tetraArray[i]->n1][tetraArray[i]->n4]) {
			springArray[nofSprings++] = new Spring(vertexArray[tetraArray[i]->n1], vertexArray[tetraArray[i]->n4], stiffness);
			adjacent[tetraArray[i]->n1][tetraArray[i]->n4] = true;
			adjacent[tetraArray[i]->n4][tetraArray[i]->n1] = true;
		}
		if (false == adjacent[tetraArray[i]->n2][tetraArray[i]->n3]) {
			springArray[nofSprings++] = new Spring(vertexArray[tetraArray[i]->n2], vertexArray[tetraArray[i]->n3], stiffness);
			adjacent[tetraArray[i]->n2][tetraArray[i]->n3] = true;
			adjacent[tetraArray[i]->n3][tetraArray[i]->n2] = true;
		}
		if (false == adjacent[tetraArray[i]->n2][tetraArray[i]->n4]) {
			springArray[nofSprings++] = new Spring(vertexArray[tetraArray[i]->n2], vertexArray[tetraArray[i]->n4], stiffness);
			adjacent[tetraArray[i]->n2][tetraArray[i]->n4] = true;
			adjacent[tetraArray[i]->n4][tetraArray[i]->n2] = true;
		}
		if (false == adjacent[tetraArray[i]->n3][tetraArray[i]->n4]) {
			springArray[nofSprings++] = new Spring(vertexArray[tetraArray[i]->n3], vertexArray[tetraArray[i]->n4], stiffness);
			adjacent[tetraArray[i]->n3][tetraArray[i]->n4] = true;
			adjacent[tetraArray[i]->n4][tetraArray[i]->n3] = true;
		}
	}
	printf("Vertices %i\n", nofVertices);
	printf("Tetras   %i\n", nofTetras);
	printf("Springs  %i\n", nofSprings);
	
	fclose(f);
	return true;
}


void Tetobject::Join()
{
//	void Join(Vertex* mv, int n_mv, int *tet_index, Vector4f *bc)	
	int i=0, k=0, best=-1;
	float	curdis=0.0f, bestdis = 1e38f;
	SbVec3f mean,h;

	for (k=0; k<nofTetras; k++) //Update all Matrices in Tetras
		tetraArray[k]->UpdateMatrix();

	for (i=0; i<mesh.nofMeshVertices; i++)	//Preprocess - not optimized
	{
		curdis=0.0f;
		bestdis = 1e38f;
		best = -1;
		//linear search through tetras to find nearest
		for (k=0; k<nofTetras; k++) 
		{
			mean =  0.25f * (vertexArray[tetraArray[k]->n1]->pos + 
						 vertexArray[tetraArray[k]->n2]->pos + 
						 vertexArray[tetraArray[k]->n3]->pos + 
						 vertexArray[tetraArray[k]->n4]->pos);
			h = mesh.meshVertexArray[i] - mean;
			curdis = h.length();
			if (curdis < bestdis)
			{
				bestdis = curdis;
				best = k;
			}
		}
		mesh.tetIndexArray[i] = best;

		//Compute barycentric coordinates 
		float v[4],b[4];
		mesh.meshVertexArray[i].getValue(v[0],v[1],v[2]);
		v[3] = 1;
		tetraArray[best]->T.SolveLinSys(b,v);
		mesh.baryCoordArray[i].setValue(b);
	}
}


void Tetobject::UpdateGraphics()
{
	int i=0;
	float h[4],s[4];
	
	//Update meshvertices using barycentric coordinates
	for (i=0; i<mesh.nofMeshVertices; i++)	
	{
		mesh.baryCoordArray[i].getValue(s[0],s[1],s[2],s[3]);
		tetraArray[mesh.tetIndexArray[i]]->T.MatVecMultiplication(s,h);
		mesh.meshVertexArray[i].setValue(h[0],h[1],h[2]);
	}
	mesh.UpdateFaceNormals();
	mesh.Render();
}



void Tetobject::UpdatePhysics(float timestep)
{
	int k=0;


	Vertex* cur;	//aux variable
	//calculate movement (using Beeman)
	
	//x(t+h) update positions
		for (int i=0; i<nofVertices; i++) {
			cur = vertexArray[i];
			cur->pos = cur->pos + timestep*cur->velo +
				timestep*timestep*(2.0f/3.0f*cur->acc - 1.0f/6.0f*cur->lastacc);
		}

	//a(t+h) uptdate forces
		//set forces zero
		for(int i=0; i < nofVertices; i++) {
			vertexArray[i]->forcezero();
		}

		//iterate over springs
		for(int i = 0; i < nofSprings; i++) {
			springArray[i]->Updatepoints();
		}

		//Add driver forces
		driver->UpdatePoints();

		//add external forces
		//if any...

		//damping
		SbVec3f meanVelo(0.0f, 0.0f, 0.0f);		// calculate mean velocity of all mass points
		for(int i=0; i<nofVertices; i++) {
			meanVelo += vertexArray[i]->velo;
		}
		meanVelo /= (float)nofVertices;
		SbVec3f relative;
		for(int i=0; i<nofVertices; i++) {
			relative = vertexArray[i]->velo - meanVelo;
			vertexArray[i]->force -= relative*0.15f;
			vertexArray[i]->velo = vertexArray[i]->velo*0.999f;
		}

		
		//calculate a
		for(int i=0; i < nofVertices; i++) {
			cur = vertexArray[i];
			cur->last2acc = cur->lastacc;
			cur->lastacc = cur->acc;
			cur->acc = cur->force/cur->mass;
		}

	//v(t+h) update velocity
		for(int i=0; i < nofVertices; i++) {
			cur = vertexArray[i];
			cur->velo = cur->velo +
				timestep * (cur->acc*5.0f/12.0f + cur->lastacc * 2.0f/3.0f
				- cur->last2acc*1.0f/12.0f);

		}


	//At the end: Update Matrices in tetras
	for (k=0; k<nofTetras; k++) //Update all Matrices in Tetras
		tetraArray[k]->UpdateMatrix();
}

void Tetobject::RenderTetras()
{
	int i=0;
	glBegin(GL_LINES);
		glColor3f(1,0,0);
		for (i=0; i<nofTetras; i++)
		{
			glVertex3fv(tetraArray[i]->p1->pos.getValue());
			glVertex3fv(tetraArray[i]->p2->pos.getValue());
			glVertex3fv(tetraArray[i]->p1->pos.getValue());
			glVertex3fv(tetraArray[i]->p3->pos.getValue());
			glVertex3fv(tetraArray[i]->p1->pos.getValue());
			glVertex3fv(tetraArray[i]->p4->pos.getValue());
			glVertex3fv(tetraArray[i]->p2->pos.getValue());
			glVertex3fv(tetraArray[i]->p3->pos.getValue());
			glVertex3fv(tetraArray[i]->p2->pos.getValue());
			glVertex3fv(tetraArray[i]->p4->pos.getValue());
			glVertex3fv(tetraArray[i]->p3->pos.getValue());
			glVertex3fv(tetraArray[i]->p4->pos.getValue());
		}
	glEnd();
	RenderRefPoints();
}

void Tetobject::RenderRefPoints()
{
	int i=0;
		glPointSize(8.0);
		glBegin(GL_POINTS);
		SbVec3f tmp;
		glColor3f(1,1,0);
		for (i=0; i<8; i++)
		{
			glVertex3fv(driver->quader[i]->pos.getValue());
		}
	glEnd();
}

void Tetobject::BlowUp()
{
	//Increases all spring inital lenghts by 10 percent
	for(int i = 0; i < nofSprings; i++)
	{
		springArray[i]->BlowUp();
	};
}
