#include <math.h>
#include <iostream>
#include "constants.h"
#include "vector.h"
#include "triangle.h"
#define GLUT_DISABLE_ATEXIT_HACK 
#include <GL/glut.h>

using namespace std;

/*
 * Implement the Triangle class
 */

// Default constructor
Triangle::Triangle(void):color(COLORR, COLORG, COLORB)
{}

// Constructor sets vertex locations
Triangle::Triangle(const Vector& _v1, const Vector& _v2, const Vector& _v3):color(COLORR, COLORG, COLORB)
{
	verts = new Vector[3];

	verts[0] = _v1;
	verts[1] = _v2;
	verts[2] = _v3;

	sverts = new Vector[3];

	sverts[0] = Vector(5,-1,10);
	sverts[1] = Vector(1,1,1);
	sverts[2] = Vector(2,3,4);

	Vector edge1to2 = _v2 - _v1;
	Vector edge1to3 = _v3 - _v1;
	normal = edge1to2.cross(edge1to3);

	standardize();
}

Triangle::Triangle(const Triangle& src):color(COLORR, COLORG, COLORB)
{
	normal = src.normal;
	color = src.color;
	verts = new Vector[3];
	sverts = new Vector[3];

	for (int i = 0; i < 3; ++i)
	{
		verts[i] = src.verts[i];
		sverts[i] = src.sverts[i];
	}
	for (int i = 0; i < 16; ++i) tmatrix[i] = src.tmatrix[i];
}

Triangle::~Triangle(void)
{
	delete[] verts;
	delete[] sverts;
}

/*
 * Assignment operator for Triangle.
 */
Triangle& Triangle::operator= (const Triangle& src)
{
	normal = src.normal;
	color = src.color;
	verts = new Vector[3];
	sverts = new Vector[3];
	
	for (int i = 0; i < 3; ++i)
	{
		verts[i] = src.verts[i];
		sverts[i] = src.sverts[i];
	}
	for (int i = 0; i < 16; ++i)
	{
		tmatrix[i] = src.tmatrix[i];
	}
}

void Triangle::draw()
{
	glColor3f(color[0], color[1], color[2]);
	glBegin(GL_TRIANGLES);
	glVertex3f(verts[0][0], verts[0][1], verts[0][2]);
	glVertex3f(verts[1][0], verts[1][1], verts[1][2]);
	glVertex3f(verts[2][0], verts[2][1], verts[2][2]);
	glEnd();
	glCullFace(GL_FRONT);
	glColor3f(1, 1, 1);
	glBegin(GL_TRIANGLES);
	glVertex3f(verts[0][0], verts[0][1], verts[0][2]);
	glVertex3f(verts[1][0], verts[1][1], verts[1][2]);
	glVertex3f(verts[2][0], verts[2][1], verts[2][2]);
	glEnd();
	glCullFace(GL_BACK);
}

// Draw triangle with preset texturing
// Scale the texture so that it doesn't repeat, by dividing by
// MAP_SIZE
// Also make low areas darker than high areas by giving triangles
// altitude-dependent colors and letting color show through the 
// texture.
// 911 - this function breaks encapsulation since the triangle knows
// that it's dealing with terrains.
void Triangle::textureDraw()
{
	glBegin(GL_TRIANGLES);
	glTexCoord2f(verts[0][0] / MAP_SIZE, verts[0][2] / MAP_SIZE);
	glColor3f(verts[0][1] / TERRAIN_HEIGHT, verts[0][1] / TERRAIN_HEIGHT, verts[0][1] / TERRAIN_HEIGHT);
	glVertex3f(verts[0][0], verts[0][1], verts[0][2]);
	glTexCoord2f(verts[1][0] / MAP_SIZE, verts[1][2] / MAP_SIZE);
	glColor3f(verts[1][1] / TERRAIN_HEIGHT, verts[1][1] / TERRAIN_HEIGHT, verts[1][1] / TERRAIN_HEIGHT);
	glVertex3f(verts[1][0], verts[1][1], verts[1][2]);
	glTexCoord2f(verts[2][0] / MAP_SIZE, verts[2][2] / MAP_SIZE);
	glColor3f(verts[2][1] / TERRAIN_HEIGHT, verts[2][1] / TERRAIN_HEIGHT, verts[2][1] / TERRAIN_HEIGHT);
	glVertex3f(verts[2][0], verts[2][1], verts[2][2]);
	glEnd();
}

void Triangle::setColor(Vector col)
{
	color = col;
}

// Translate all vertices by provided vector
void Triangle::translate(Vector tvec)
{
	verts[0] += tvec;
	verts[1] += tvec;
	verts[2] += tvec;
	standardize();
}

// Print out locations of vertices
void Triangle::print(void)
{
	cout << "Vertices:" << endl;
	verts[0].print();
	verts[1].print();
	verts[2].print();
	cout << "Standardized vertices:" << endl;
	sverts[0].print();
	sverts[1].print();
	sverts[2].print();
}
// Standardize the triangle so that it has one vertex at the 
// origin, one on the positive x-axis, and one in positive
// z-space. This is used for collision detection. Note that
// the results are stored in the sverts array.

void Triangle::standardize()
{

	//  we assume that the triangle is valid;  i.e. the vertices are
	//  distinct and non-collinear -- if not this will BOMB!
	//  you need to take care of this somewhere

	// Taking care of this here, then. We'll never actually collide with this triangle so
	// it's not a problem if we just return out. Note that the "vertices are distinct"
	// problem is a subset of the "vertices are collinear" problem. Note also that
	// checking for equality on vectors involves the use of the EPSILON constant.
 	if (collinear(verts[0], verts[1], verts[2])) return;

	int i,j,k;			// loop indices (because this was written under vc6
						// which has funky scoping rules
	float sigma, phi;	// angles of rotation
	Vector v(0,1,0);		// axis of rotation, initially the y axis
	Vector x(1,0,0);		// x axis

	glPushMatrix();			// save the current matrix

	// Copy the triangle's vertices.
	Vector standard[3];
	for (i = 0; i < 3; ++i)
	{
		standard[i] = this->verts[i];
	}

	// translate so the first vertex is at the origin
	standard[1] -= standard[0];
	standard[2] -= standard[0];
	standard[0] = Vector(0,0,0);

	// rotate to align last edge with the x axis
	Vector w = standard[2];				// set w to the last edge
	w = w.normalize();						// normalize it
	if (w != x && w!= -x) {				// check that is not already on the x-axis

		// find the vector of rotation
		v = w.cross(x);			
		v = v.normalize();					

		// find the angle of rotation in degrees
		phi = (float) acos((double) w.dot(x));
		phi *= 360.f/(2.0f*PI);	

		// have openGL compute the rotation transform and read it into myMatrix
		glLoadIdentity();				
		glRotatef(phi,v[0],v[1],v[2]);	
		glGetFloatv(GL_MODELVIEW_MATRIX,tmatrix);
									

		// update second and third vertex -- first won't change
		for (i=1; i<=2; i++) { 
			Vector tmp(0,0,0); 
			for (j=0; j<3; j++) { // for each row of the matrix 
				for (k=0; k<3; k++) {  // for each column
					tmp[j] += tmatrix[j+4*k]*standard[i][k];  // the matrix is column-major
				}
			}
			standard[i]=tmp;
		}

		// correct for rounding errors -- third vertex should lie on x axis
		standard[2][1]=0;
		standard[2][2]=0;

	}
	else {		// compute angle of  rotatation for case when the last edge already lies on the x-axis	
		if (w == x)
			phi=0;
		else
			phi = 180;
 
	}


	// rotate to bring second vertex into y=0 plane
	w=standard[1];			// set w to the first edge
	//project w into the x=0 plane
	w[0]=0.0f;

	// compute the angle of rotation in degrees
	sigma = acos((w[2]/w.length()));
	sigma *= 360.f/(2.0f*PI);

	// axis of rotation is either +x or -x
	if (w[1]<0)			
		x *= -1.0f;

	// have openGL compute the transform and read it into myMatrix
	glLoadIdentity();	
	glRotatef(sigma,x[0],x[1],x[2]);
	glGetFloatv(GL_MODELVIEW_MATRIX,tmatrix);


	// update the second vertex -- the first and third won't change
	Vector tmp(0,0,0); 
	for (j=0; j<3; j++) { // for each row of the matrix 
		for (k=0; k<3; k++) {  // for each column
			tmp[j] += tmatrix[j+4*k]*standard[1][k];  // the matrix is column-major
			}
		}
	standard[1]=tmp;

	// correct for roundoff errors
	standard[1][1]=0;

	// now compute the single forward (standardizing) transform
	glLoadIdentity();
	glRotatef(sigma,x[0],x[1],x[2]);
	glRotatef(phi,v[0],v[1],v[2]);
	glTranslatef(-this->verts[0][0],-this->verts[0][1], -this->verts[0][2]);
	glGetFloatv(GL_MODELVIEW_MATRIX,tmatrix);



	glPopMatrix(); // restore current matrix

	for (int k=0;k <3; k++)
	{
		// compute kth vertex
		for (int i=0;i<3;i++) 
		{
			sverts[k][i] = 0;
			for (int j=0;j<3;j++)
				sverts[k][i] += tmatrix[4*j+i]*verts[k][j];
			sverts[k][i] += tmatrix[12+i];
		}
	}
}