#include <iostream>

using namespace std;

#pragma warning(disable:4244)
#include <math.h>

#include "ccol.h"

extern "C"{
	bool C_checkTriangle(float *,float *,float *,float *);
}

bool isBackCulling=false;

VECTOR::VECTOR(){x=0; y=0; z=0;}
VECTOR::VECTOR(float x, float y, float z){this->x=x; this->y=y; this->z=z;}

void VECTOR::set(float x, float y, float z){this->x=x; this->y=y; this->z=z;}
void VECTOR::get(float **floatvector){(*floatvector)[0]=x; (*floatvector)[1]=y; (*floatvector)[2]=z;}

VECTOR VECTOR::operator+(VECTOR const &V1)
{
	return VECTOR(V1.x+x,V1.y+y,V1.z+z);
}

VECTOR VECTOR::operator-(VECTOR const &V1)
{
	return VECTOR(x-V1.x,y-V1.y,z-V1.z);
}

VECTOR VECTOR::operator*(VECTOR const &V1)
{
	return VECTOR(x*V1.x,y*V1.y,z*V1.z);
}

VECTOR VECTOR::operator/(VECTOR const &V1)
{
	return VECTOR(x/V1.x,y/V1.y,z/V1.z);
}

VECTOR VECTOR::operator*(float const &c)
{
    return VECTOR(x*c,y*c,z*c); 
}

VECTOR VECTOR::operator*(double const &c)
{
    return VECTOR(x*c,y*c,z*c); 
}

VECTOR VECTOR::operator/(float const &c)
{
    float Invers=1.0f/c;//only divide one time!
    return VECTOR(x*Invers,y*Invers,z*Invers); 
}

float VECTOR::length()
{
    return (float)sqrt(x*x+y*y+z*z);
}

float VECTOR::squaredLength()
{
	return (float)x*x+y*y+z*z;
}

void VECTOR::SetLength(float c)
{
	float temp=length();
	x *= c/temp;
	y *= c/temp;
	z *= c/temp;
}

void VECTOR::normalize()
{
    float dist=1/length();//find the distance, and inverse it

    x *= dist;
    y *= dist;
    z *= dist;
}

float VECTOR::dot(VECTOR V)
{
    return V.x*x+V.y*y+V.z*z;
}

VECTOR VECTOR::cross(VECTOR V)
{
    return VECTOR((y*V.z-z*V.y),(z*V.x-x*V.z),(x*V.y-y*V.x));
}

PLANE::PLANE(VECTOR origin, VECTOR normal) 
{
	this->normal = normal;
	this->origin = origin;
	equation[0] = normal.x;
	equation[1] = normal.y;
	equation[2] = normal.z;
	equation[3] = -(normal.x*origin.x+normal.y*origin.y+normal.z*origin.z);
}

// Construct from triangle:
PLANE::PLANE(VECTOR p1,VECTOR p2,VECTOR p3)
{
	normal = (p2-p1).cross(p3-p1);
	normal.normalize();
	origin = p1;
	equation[0] = normal.x;
	equation[1] = normal.y;
	equation[2] = normal.z;
	equation[3] = -(normal.x*origin.x+normal.y*origin.y+normal.z*origin.z);
}

bool PLANE::isFrontFacingTo(VECTOR direction) 
{
	double dot = normal.dot(direction);
	return (dot <= 0);
}

double PLANE::signedDistanceTo(VECTOR point) 
{
	return (point.dot(normal)) + equation[3];
}

bool getLowestRoot(float a, float b, float c, float maxR,float* root) 
{
	// Check if a solution exists
	float determinant = b*b - 4.0f*a*c;
	
	// If determinant is negative it means no solutions.
	if (determinant < 0.0f) return false;
	
	// calculate the two roots: (if determinant == 0 then
	// x1==x2 but let's disregard that slight optimization)
	float sqrtD = (float)sqrt(determinant);
	float r1 = (-b - sqrtD) / (2*a);
	float r2 = (-b + sqrtD) / (2*a);
	
	// Sort so x1 <= x2
	if (r1 > r2) 
	{
		float temp = r2;
		r2 = r1;
		r1 = temp;
	}
	
	// Get lowest root:
	if (r1 > 0 && r1 < maxR) 
	{
		*root = r1;
		return true;
	}
	
	// It is possible that we want x2 - this can happen
	// if x1 < 0
	if (r2 > 0 && r2 < maxR) 
	{
		*root = r2;
		return true;
	}
	
	// No (valid) solutions
	return false;
}

typedef unsigned int uint32;
#define in(a) ((uint32&) a)

bool checkPointInTriangle(VECTOR point,VECTOR pa,VECTOR pb,VECTOR pc)
{
	VECTOR e10=pb-pa;
	VECTOR e20=pc-pa;
	
	float a = e10.dot(e10);
	float b = e10.dot(e20);
	float c = e20.dot(e20);
	float ac_bb=(a*c)-(b*b);
	
	VECTOR vp(point.x-pa.x, point.y-pa.y, point.z-pa.z);
	
	float d = vp.dot(e10);
	float e = vp.dot(e20);
	float x = (d*c)-(e*b);
	float y = (e*a)-(d*b);
	float z = x+y-ac_bb;
	return (( in(z)& ~(in(x)|in(y)) ) & 0x80000000);
}

//colPackage

//in - velocity 0,1,2
//in - normalizedVelocity 3,4,5
//in - basePoint 6,7,8

//out - foundCollision 9
//out - nearestDistance (static) 10
//out - intersectionPoint 11,12,13

//Sphere->triangle collision
// Assumes: p1,p2 and p3 are given in ellipsoid space:
bool C_checkTriangle(float *col_pack,float *aa,float *bb,float *cc)
{
	VECTOR velocity_cp(col_pack[0],col_pack[1],col_pack[2]);
	VECTOR normalizedVelocity_cp(col_pack[3],col_pack[4],col_pack[5]);
	VECTOR basePoint_cp(col_pack[6],col_pack[7],col_pack[8]);
	
	float foundCollision_cp=col_pack[9];
	float nearestDistance_cp=col_pack[10];
	VECTOR intersectionPoint_cp(col_pack[11],col_pack[12],col_pack[13]);
	
	VECTOR p1(aa[0],aa[1],aa[2]);
	VECTOR p2(bb[0],bb[1],bb[2]);
	VECTOR p3(cc[0],cc[1],cc[2]);

	// Make the plane containing this triangle.
	PLANE trianglePlane(p1,p2,p3);

	/*
	// Is triangle front-facing to the velocity vector?
	// We only check front-facing triangles
	// (your choice of course)
	if( !trianglePlane.isFrontFacingTo(normalizedVelocity_cp)) //&& (!isBackCulling))
	{
		trianglePlane.equation[0] *=-1;
		trianglePlane.equation[1] *=-1;
		trianglePlane.equation[2] *=-1;
		trianglePlane.equation[3] *=-1;

		trianglePlane.normal.x*=-1;
		trianglePlane.normal.y*=-1;
		trianglePlane.normal.z*=-1;
	}
	*/

	if (trianglePlane.isFrontFacingTo(normalizedVelocity_cp)) //|| (! isBackCulling)) 
	{
		// Get interval of plane intersection:
		double t0, t1;
		bool embeddedInPlane = false;
	
		// Calculate the signed distance from sphere
		// position to triangle plane
		double signedDistToTrianglePlane =	trianglePlane.signedDistanceTo(basePoint_cp);
		
		// cache this as we're going to use it a few times below:
		float normalDotVelocity =	trianglePlane.normal.dot(velocity_cp);
		
		// if sphere is travelling parrallel to the plane:
		if (normalDotVelocity == 0.0f) 
		{
			if (fabs(signedDistToTrianglePlane) >= 1.0f) 
			{
				// Sphere is not embedded in plane.
				// No collision possible:
				return 0;
			}
			else 
			{
				// sphere is embedded in plane.
				// It intersects in the whole range [0..1]
				embeddedInPlane = true;
				t0 = 0.0;
				t1 = 1.0;
			}
		}
		else 
		{
			// N dot D is not 0. Calculate intersection interval:
			t0=(-1.0-signedDistToTrianglePlane)/normalDotVelocity;
			t1=( 1.0-signedDistToTrianglePlane)/normalDotVelocity;
		
			// Swap so t0 < t1
			if (t0 > t1) {
				double temp = t1;
				t1 = t0;
				t0 = temp;
			}
			
			// Check that at least one result is within range:
			if (t0 > 1.0f || t1 < 0.0f) 
			{
				// Both t values are outside values [0,1]
				// No collision possible:
				return 0;
			}
			
			// Clamp to [0,1]
			if (t0 < 0.0) t0 = 0.0;
			if (t1 < 0.0) t1 = 0.0;
			if (t0 > 1.0) t0 = 1.0;
			if (t1 > 1.0) t1 = 1.0;
		}

		// OK, at this point we have two time values t0 and t1
		// between which the swept sphere intersects with the
		// triangle plane. If any collision is to occur it must
		// happen within this interval.
		VECTOR collisionPoint;
		bool foundCollison = false;
		float t = 1.0;

		// First we check for the easy case - collision inside
		// the triangle. If this happens it must be at time t0
		// as this is when the sphere rests on the front side
		// of the triangle plane. Note, this can only happen if
		// the sphere is not embedded in the triangle plane.		
		if (!embeddedInPlane) 
		{
			VECTOR planeIntersectionPoint =	(basePoint_cp-trianglePlane.normal) + velocity_cp*t0;
			if (checkPointInTriangle(planeIntersectionPoint,p1,p2,p3))
			{
				foundCollison = true;
				t = t0;
				collisionPoint = planeIntersectionPoint;
			}
		}

		// if we haven't found a collision already we'll have to
		// sweep sphere against points and edges of the triangle.
		// Note: A collision inside the triangle (the check above)
		// will always happen before a vertex or edge collision!
		// This is why we can skip the swept test if the above
		// gives a collision!
		if (foundCollison == false) 
		{
			// some commonly used terms:
			VECTOR velocity = velocity_cp;
			VECTOR base = basePoint_cp;
			float velocitySquaredLength = velocity.squaredLength();
			float a,b,c; // Params for equation
			float newT;
		
			// For each vertex or edge a quadratic equation have to
			// be solved. We parameterize this equation as
			// a*t^2 + b*t + c = 0 and below we calculate the
			// parameters a,b and c for each test.
			// Check against points:
			a = velocitySquaredLength;
			
			// P1
			b = 2.0*(velocity.dot(base-p1));
			c = (p1-base).squaredLength() - 1.0;
			if (getLowestRoot(a,b,c, t, &newT)) 
			{
				t = newT;
				foundCollison = true;
				collisionPoint = p1;
			}
			
			// P2
			b = 2.0*(velocity.dot(base-p2));
			c = (p2-base).squaredLength() - 1.0;
			if (getLowestRoot(a,b,c, t, &newT)) 
			{
				t = newT;
				foundCollison = true;
				collisionPoint = p2;
			}
			
			// P3
			b = 2.0*(velocity.dot(base-p3));
			c = (p3-base).squaredLength() - 1.0;
			if (getLowestRoot(a,b,c, t, &newT)) 
			{
				t = newT;
				foundCollison = true;
				collisionPoint = p3;
			}
			
			// Check agains edges:
			// p1 -> p2:
			VECTOR edge = p2-p1;
			VECTOR baseToVertex = p1 - base;
			float edgeSquaredLength = edge.squaredLength();
			float edgeDotVelocity = edge.dot(velocity);
			float edgeDotBaseToVertex = edge.dot(baseToVertex);
			
			// Calculate parameters for equation
			a = edgeSquaredLength*-velocitySquaredLength +	edgeDotVelocity*edgeDotVelocity;
			b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex;
			c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex;

			// Does the swept sphere collide against infinite edge?
			if (getLowestRoot(a,b,c, t, &newT)) 
			{
				// Check if intersection is within line segment:
				float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength;

				if (f >= 0.0 && f <= 1.0) 
				{
					// intersection took place within segment.
					t = newT;
					foundCollison = true;
					collisionPoint = p1 + edge*f;
				}
			}

			// p2 -> p3:
			edge = p3-p2;
			baseToVertex = p2 - base;
			edgeSquaredLength = edge.squaredLength();
			edgeDotVelocity = edge.dot(velocity);
			edgeDotBaseToVertex = edge.dot(baseToVertex);
			a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity;
			b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex;
			c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex;
			if (getLowestRoot(a,b,c, t, &newT)) 
			{
				float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength;
				
				if (f >= 0.0 && f <= 1.0) 
				{
					t = newT;
					foundCollison = true;
					collisionPoint = p2 + edge*f;
				}
			}

			// p3 -> p1:
			edge = p1-p3;
			baseToVertex = p3 - base;
			edgeSquaredLength = edge.squaredLength();
			edgeDotVelocity = edge.dot(velocity);
			edgeDotBaseToVertex = edge.dot(baseToVertex);
			a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity;
			b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex;
			c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex;
			if (getLowestRoot(a,b,c, t, &newT)) 
			{
				float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength;
				
				if (f >= 0.0 && f <= 1.0) 
				{
					t = newT;
					foundCollison = true;
					collisionPoint = p3 + edge*f;
				}
			}
		}

		// Set result:
		if (foundCollison == true) 
		{
			// distance to collision: 't' is time of collision
			float distToCollision = t*velocity_cp.length();

			// Does this triangle qualify for the closest hit?
			if (distToCollision < nearestDistance_cp)
			{
				// Collision information nessesary for sliding
				nearestDistance_cp = distToCollision;
				intersectionPoint_cp=collisionPoint;
				foundCollision_cp = true;

				col_pack[9]=foundCollision_cp;
				col_pack[10]=nearestDistance_cp;
				col_pack[11]=intersectionPoint_cp.x;
				col_pack[12]=intersectionPoint_cp.y;
				col_pack[13]=intersectionPoint_cp.z;
				
				return 1;
			}
		}
	} // if not backface
}

