#include <windows.h>
#include <gl/gl.h>
#include "obb.h"
#include "../math/math.h"
#include "../math/plane.h"
#include "../math/triangle.h"
#include "../global.h"
#include <float.h>

GcOBB::GcOBB()
{
	m_orthogonal[0] = GcVector3(1, 0, 0);
	m_orthogonal[1] = GcVector3(0, 1, 0);
	m_orthogonal[2] = GcVector3(0, 0, 1);
}

GcOBB::GcOBB( GcVector3 & extents )
{
	m_extents = extents;
}

GcOBB::GcOBB( GcVector3 & extents, GcVector3 & position )
{
	m_extents = extents;
	SetTranslation(position);
}


void GcOBB::RenderOutlines()
{
	glPushMatrix();
		
		
		/*glTranslatef(	GetWorldTranslation().x ,
						GetWorldTranslation().y,
		
				GetWorldTranslation().z);*/
	
		Matrix4 tmp = GetWorldMatrix();
		//tmp.SetIdentity();
		//tmp.SetTranslation( GetWorldMatrix().GetTranslation() );
		glMultMatrixf(tmp);
		

		glLineWidth(2);
		glColor3f(0.5f, 0.0f, 0.0f);
		
		glBegin(GL_LINES);
			
			// Front top
			glVertex3f(-m_extents.x, m_extents.y, -m_extents.z);
			glVertex3f(m_extents.z, m_extents.y, -m_extents.z);
			
			// Back top
			glVertex3f(-m_extents.x, m_extents.y, m_extents.z );
			glVertex3f(m_extents.z, m_extents.y, m_extents.z);

			// Left top
			glVertex3f(-m_extents.x, m_extents.y, -m_extents.z);
			glVertex3f(-m_extents.x, m_extents.y, m_extents.z );

			// Right top
			glVertex3f(m_extents.x, m_extents.y, -m_extents.z);
			glVertex3f(m_extents.x, m_extents.y, m_extents.z );

			
			
			// Front bottom
			glVertex3f(-m_extents.x, -m_extents.y, -m_extents.z);
			glVertex3f(m_extents.z, -m_extents.y, -m_extents.z);
			
			// Back bottom
			glVertex3f(-m_extents.x, -m_extents.y, m_extents.z );
			glVertex3f(m_extents.z, -m_extents.y, m_extents.z);

			// Left bottom
			glVertex3f(-m_extents.x, -m_extents.y, -m_extents.z);
			glVertex3f(-m_extents.x, -m_extents.y, m_extents.z );

			// Right bottom
			glVertex3f(m_extents.x, -m_extents.y, -m_extents.z);
			glVertex3f(m_extents.x, -m_extents.y, m_extents.z );


			
			
			// Front left
			glVertex3f(-m_extents.x, m_extents.y, -m_extents.z);
			glVertex3f(-m_extents.x, -m_extents.y, -m_extents.z);

			// Front right
			glVertex3f(m_extents.z, m_extents.y, -m_extents.z);
			glVertex3f(m_extents.z, -m_extents.y, -m_extents.z);

			// Back left
			glVertex3f(-m_extents.x, m_extents.y, m_extents.z);
			glVertex3f(-m_extents.x, -m_extents.y, m_extents.z);

			// Back right
			glVertex3f(m_extents.z, m_extents.y, m_extents.z);
			glVertex3f(m_extents.z, -m_extents.y, m_extents.z);

		glEnd();

		glLineWidth(1);
	
	glPopMatrix();
}


const bool GcOBB::Intersects(const GcPlane & plane)
{
	glPushMatrix();
	
	GcVector3 point(400.0f, 520.0f, 500.0f);
	glTranslatef( point.x, point.y, point.z );

	glPointSize(5);
	
	glColor3f(0.5f, 0.0f, 0.0f);

	glBegin(GL_POINTS);
		//glVertex3fv( point );
		glVertex3f(0.0f, 0.0f, 0.0f);
		//glVertex3f(500.0f, 0.0f, 0.0f);
	glEnd();
	
	GcVector3 normal(0, 1, 0);
	normal *= 30;

	glColor3f(0.5f, 0.0f, 0.0f);
	glBegin(GL_LINES);
		//glVertex3fv( point );
		glVertex3f(0.0f, 0.0f, 0.0f);
		glVertex3fv(normal);
		//glVertex3f(500.0f, 0.0f, 0.0f);
	glEnd();
	
	glPopMatrix();


	GcVector3 & extents = m_extents;
	GcVector3 & xAxis = m_worldMatrix.Axis(0);
	GcVector3 & yAxis = m_worldMatrix.Axis(1);
	GcVector3 & zAxis = m_worldMatrix.Axis(2);

/*	float r = ( ( normal.DotProduct(xAxis) ) * extents.x ) + ( (normal.DotProduct(yAxis)) * extents.y ) + ( (normal.DotProduct(zAxis)) * extents.z );
	//GcVector3 PC = point - GetWorldTranslation();
	float distance = plane.Distance( GetWorldTranslation() );
	distance = fabs(distance);
	//float distance = plane.Distance( GcVector3(0, 0 ,0) );
*/
	
	

	
	
	GcVector3 distance;
	distance.x = extents.x * ( plane.m_normal * xAxis );
	distance.y = extents.y * ( plane.m_normal * yAxis );
	distance.z = extents.z * ( plane.m_normal * zAxis );

	float radius = fabs( distance.x ) + fabs( distance.y ) + fabs( distance.z );

	float distanceCenterToPlane = plane.Distance( GetWorldTranslation() );

	//dbgC.Write("Distance to plane: %f\n", distanceCenterToPlane);
	//dbgC.Write("radius: %f\n", radius);

	if( fabs(distanceCenterToPlane) <= radius )
		return true;
/*


	NeMatrix kRotMat = GetWorldRotation().ToMatrix();

	NeVector3d kDist( m_kDim.x * ( rkPlane.m_kNormal * kRotMat.GetColumn(0) ),
					  m_kDim.y * ( rkPlane.m_kNormal * kRotMat.GetColumn(1) ),
					  m_kDim.z * ( rkPlane.m_kNormal * kRotMat.GetColumn(2) ) );

	float fRadius = fabs( kDist.x ) + fabs( kDist.y ) + fabs( kDist.z );

	float fCenterDistToPlane = rkPlane.Distance( GetWorldPosition() );

	return( fabs( fCenterDistToPlane ) <= fRadius ); 
*/
	glPointSize(1);
	
	return false;
}

const bool GcOBB::Intersects(const GcTriangle & triangle)
{
	// A total 13 axes will be tested
	// 1) x1 The normal of the triangle
	// 2) x3 The faces of the OBB
	// 3) x9 The cross products of the triangle - box edges
	

	float triMin, triMax, boxMin, boxMax;
	GcVector3 A; // Will hold a given arbitrary axis
	GcVector3 triEdge[3];	// Will hold the edges of the triangle


	// Compute triangle edges
	triEdge[0] = triangle.b - triangle.a;
	triEdge[1] = triangle.c - triangle.a;
	

	// Test direction of triangle normal
	A = triangle.Normal();

	triMin = A.DotProduct( triangle.a );
	triMax = triMin;
	
	GcMath::ProjectOBB(A, *this, boxMin, boxMax);
	
	// The projected box and triangle do not overlap on the A axis, return negative
	if( boxMax < triMin || triMax < boxMin )
		return false;

	int i;
	
	GcVector3 center = GetWorldTranslation();

	// Test the directions of the faces
	for( i = 0; i < 3; i++ )
	{
		A = this->Axis(i);
		
		GcMath::ProjectTriangle( A, triangle, triMin, triMax );
		
		float centerOnA = A.DotProduct( center );
		boxMin = centerOnA - this->Extent(i);
		boxMax = centerOnA + this->Extent(i);
		
		if( boxMax < triMin || triMax < boxMin )
			return false;
	}

	// Test the cross products of the triangle - box edges
	triEdge[2] = triEdge[1] - triEdge[0];
	
	for( i = 0; i < 3; i++ )
	{
		for( int j = 0; j < 3; j++)
		{
			A = triEdge[i].CrossProduct( this->Axis(j) );

			GcMath::ProjectTriangle( A, triangle, triMin, triMax );
			GcMath::ProjectOBB( A, *this, boxMin, boxMax );
			
			if( boxMax < triMin || triMax < boxMin )
				return false;
		}
	}

	return true;
}

const bool GcOBB::Intersects( GcAABB & box )
{
	return Intersects( GcOBB( box.GetExtents(), box.GetWorldTranslation() ) );
}


const bool GcOBB::Intersects( GcOBB & box )
{
	float R[3][3];
	float ra, rb, t;
	long i, k;


	GcVector3 v = box.GetWorldTranslation() - GetWorldTranslation();

	// Translation to B with A as reference
	GcVector3 T( v.DotProduct( m_worldMatrix.Axis( 0 ) ), v.DotProduct( m_worldMatrix.Axis( 1 ) ), v.DotProduct( m_worldMatrix.Axis( 2 ) ) );

	//B's basis with respect to A's local frame

	GcVector3 & extentsA = m_extents;
	GcVector3 & extentsB = box.Extents();

	// Calculate rotation matrix
	/*for( i = 0; i < 3; ++i )
		for( k = 0; k < 3; ++k )
			R[i][k] = m_orthogonal[i].DotProduct(box->GetOrthogonal()[k]);*/
	
	for( i = 0; i < 3; ++i )
	{
		for( k = 0; k < 3; ++k )
		{
			GcVector3 & A = m_worldMatrix.Axis(i);
			GcVector3 & B = box.GetWorldMatrix().Axis(k);
				
			//g_Debug->Log("Axis %d %d - %f %f %f\n", i, k, A.x, A.y, A.z);
			//g_Debug->Log("Axis %d %d - %f %f %f\n", i, k, B.x, B.y, B.z);
			//A = m_worldMatrix.GetAxis(i);
			//B = box->GetWorldMatrix().GetAxis(k);

			R[i][k] = A.DotProduct( B );
			//g_Debug->Log("---------\n");
		}
		
		
	}

	Matrix3 mat, matA, matB;
	/*mat = box->GetWorldMatrix();*/
	matA = m_worldMatrix;
	matB = box.GetWorldMatrix();
	/*
	//mat.SetTranslation( GcVector3(0, 0, 0) );
	// It's a rotation matrix - no need do the whole matrix inverse calculations**/
	mat.Transpose();

	mat = matA * mat;



	for( i = 0; i < 3; i++ )
	{
		ra = m_extents.array[i];
		//rb = box->Extents().array[0] * fabs(R[i][0]) + box->Extents().array[1] * fabs(R[i][1]) + box->Extents().array[2] * fabs(R[i][2]);
		rb = box.Extent(0) * fabs(R[i][0]) + box.Extent(1) * fabs(R[i][1]) + box.Extent(2) * fabs(R[i][2]);
		
		t = fabs( T.array[i] );

		if( t > ra + rb )
		{
			return false;
		}
	}

	// Test using the other box's axes
	

	for( k = 0; k < 3; k++ )
	{
		ra = m_extents.array[0] * fabs(R[0][k]) + m_extents.array[1] * fabs(R[1][k]) + m_extents.array[2] * fabs(R[2][k]);
		rb = box.Extents().array[k];

		t =	fabs( T.array[0]*R[0][k] + T.array[1]*R[1][k] +	T.array[2]*R[2][k] );

		if( t > ra + rb )
		{
			return false;
		}
	}

	// L = A0 x B0
	ra = extentsA.y * mat.a20 + extentsA.z * mat.a10;
	rb = extentsB.y * mat.a02 + extentsB.z * mat.a01;

	t = fabs( T.z * mat.a10 - T.y * mat.a20 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("1\n");
		return false;
	}

	// L = A0 x B1
	ra = extentsA.y * mat.a21 + extentsA.z * mat.a11;
	rb = extentsB.x * mat.a02 + extentsB.z * mat.a00;

	t =	fabs( T.z * mat.a11 - T.y * mat.a21 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("2\n");
		return false;
	}

	// L = A0 x B2
	ra = extentsA.y * mat.a22 + extentsA.z * mat.a12;
	rb = extentsB.x * mat.a01 + extentsB.y * mat.a00;

	t = fabs( T.z * mat.a12 - T.y * mat.a22 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("3\n");
		return false;
	}

	// L = A1 x B0
	ra = extentsA.x * mat.a20 + extentsA.z * mat.a00;
	rb = extentsB.y * mat.a12 + extentsB.z * mat.a11;

	t = fabs( T.x * mat.a20 - T.z * mat.a00 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("4\n");
		return false;
	}

	// L = A1 x B1
	ra = extentsA.x * mat.a21 + extentsA.z * mat.a01;
	rb = extentsB.x * mat.a12 + extentsB.z * mat.a10;

	t = fabs( T.x * mat.a21 - T.z * mat.a01 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("5\n");
		return false;
	}

	// L = A1 x B2
	ra = extentsA.x * mat.a22 + extentsA.z * mat.a02;
	rb = extentsB.x * mat.a11 + extentsB.y * mat.a10;

	t = fabs( T.x * mat.a22 - T.z * mat.a02 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("6\n");
		return false;
	}

	// L = A2 x B0
	ra = extentsA.x * mat.a10 + extentsA.y * mat.a00;
	rb = extentsB.y * mat.a22 + extentsB.z * mat.a21;

	t = fabs( T.y * mat.a00 - T.x * mat.a10 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("7\n");
		return false;
	}

	// L = A2 x B1
	ra = extentsA.x * mat.a11 + extentsA.y * mat.a01;
	rb = extentsB.x * mat.a22 + extentsB.z * mat.a20;

	t = fabs( T.y * mat.a01 - T.x * mat.a11 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("8\n");
		return false;
	}

	// L = A2 x B2
	ra = extentsA.x * mat.a12 + extentsA.y * mat.a02;
	rb = extentsB.x * mat.a21 + extentsB.y * mat.a20;

	t = fabs( T.y * mat.a02 - T.x * mat.a12 );

	if( t > ra + rb )
	{
		g_DebugConsole->Write("9\n");
		return false;
	}
	















	/*// L = A0 x B0
	ra = m_extents.array[1] * fabs(R[2][0]) + m_extents.array[2] * fabs(R[1][0]);
	rb = box->Extents().array[1] * fabs(R[0][2]) + box->Extents().array[2] * fabs(R[0][1]);

	t = fabs( T.array[2] * R[1][0] - T.array[1] * R[2][0] );

	if( t > ra + rb )
	{
		dbgC.Write("1\n");
		return false;
	}

	// L = A0 x B1
	ra = m_extents.array[1] * fabs(R[2][1]) + m_extents.array[2] * fabs(R[1][1]);
	rb = box->Extents().array[0] * fabs(R[0][2]) + box->Extents().array[2] * fabs(R[0][0]);

	t =	fabs( T.array[2] * R[1][1] - T.array[1] * R[2][1] );

	if( t > ra + rb )
	{
		dbgC.Write("2\n");
		return false;
	}

	// L = A0 x B2
	ra = m_extents.array[1] * fabs(R[2][2]) + m_extents.array[2] * fabs(R[1][2]);
	rb = box->Extents().array[0] * fabs(R[0][1]) + box->Extents().array[1] * fabs(R[0][0]);

	t = fabs( T.array[2] * R[1][2] - T.array[1] * R[2][2] );

	if( t > ra + rb )
	{
		dbgC.Write("3\n");
		return false;
	}

	// L = A1 x B0
	ra = m_extents.array[0] * fabs(R[2][0]) + m_extents.array[2] * fabs(R[0][0]);

	rb = box->Extents().array[1] * fabs(R[1][2]) + box->Extents().array[2] * fabs(R[1][1]);

	t = fabs( T.array[0] * R[2][0] - T.array[2] * R[0][0] );

	if( t > ra + rb )
	{
		dbgC.Write("4\n");
		return false;
	}

	// L = A1 x B1
	ra = m_extents.array[0] * fabs(R[2][1]) + m_extents.array[2] * fabs(R[0][1]);
	rb = box->Extents().array[0] * fabs(R[1][2]) + box->Extents().array[2] * fabs(R[1][0]);

	t = fabs( T.array[0] * R[2][1] - T.array[2] * R[0][1] );

	if( t > ra + rb )
	{
		dbgC.Write("5\n");
		return false;
	}

	// L = A1 x B2
	ra = m_extents.array[0] * fabs(R[2][2]) + m_extents.array[2] * fabs(R[0][2]);
	rb = box->Extents().array[0] * fabs(R[1][1]) + box->Extents().array[1] * fabs(R[1][0]);

	t = fabs( T.array[0] * R[2][2] - T.array[2] * R[0][2] );

	if( t > ra + rb )
	{
		dbgC.Write("6\n");
		return false;
	}

	// L = A2 x B0
	ra = m_extents.array[0] * fabs(R[1][0]) + m_extents.array[1] * fabs(R[0][0]);
	rb = box->Extents().array[1] * fabs(R[2][2]) + box->Extents().array[2] * fabs(R[2][1]);

	t = fabs( T.array[1] * R[0][0] - T.array[0] * R[1][0] );

	if( t > ra + rb )
	{
		dbgC.Write("7\n");
		return false;
	}

	// L = A2 x B1
	ra = m_extents.array[0] * fabs(R[1][1]) + m_extents.array[1] * fabs(R[0][1]);
	rb = box->Extents().array[0] * fabs(R[2][2]) + box->Extents().array[2] * fabs(R[2][0]);

	t = fabs( T.array[1] * R[0][1] - T.array[0] * R[1][1] );

	if( t > ra + rb )
	{
		dbgC.Write("8\n");
		return false;
	}

	// L = A2 x B2
	ra = m_extents.array[0] * fabs(R[1][2]) + m_extents.array[1] * fabs(R[0][2]);
	rb = box->Extents().array[0] * fabs(R[2][1]) + box->Extents().array[1] * fabs(R[2][0]);