#define EPSILON 0.001f

typedef struct
{
	// Passed info
	enum COLLISION_TYPE { RAY, SPHERE, ELIPSOID };
	COLLISION_TYPE type;
	float radius;
	CVector ellipsoid;
	// Returned info
	UID id;
	bool bCollisionFound;
	CVector planeNormal;
	CVector collisionPoint;
	float planeDist;
} SCollisionInfo;

CVector ClosestPointOnLine(CVector p, CVector a, CVector b)
{
	CVector result;
	CVector c,V;
	float t,d;

	// Determine t (the length of the vector from ‘a’ to ‘p’)

	// Get the lines from point a to the point p and the line a to b
	c = p - a;
	V = b - a;

	// d = length of V

	d = sqrt( pow(V.x,2) + pow(V.y,2) + pow(V.z,2));

	// normalize V

	V.Normalize();

	// Get the magnitude of c in the direction V
	t = V.DotProduct(c);

	// Check to see if ‘t’ is beyond the extents of the line segment

	if (t < 0.0f)	{ return (a); }
	if (t > d)	  {	return (b); }

	// Return the point between ‘a’ and ‘b’

	V *= t;

	result = a + V;

	return result;
}

CVector ClosestPointOnTriangle(CVector p, CVector a, CVector b, CVector c)
{
	CVector Rab, Rca, Rbc, result;
	float dab, dca, dbc, min;

	Rab = ClosestPointOnLine(p, a, b);
	Rbc = ClosestPointOnLine(p, b, c);
	Rca = ClosestPointOnLine(p, c, a);

	dab = pow((p.x-Rab.x),2) + pow((p.y-Rab.y),2) + pow((p.z-Rab.z),2);
	dbc = pow((p.x-Rbc.x),2) + pow((p.y-Rbc.y),2) + pow((p.z-Rbc.z),2);
	dca = pow((p.x-Rca.x),2) + pow((p.y-Rca.y),2) + pow((p.z-Rca.z),2);

	min = dab;
	result = Rab;

	if (dbc < min)
	{
		min = dbc;
		result = Rbc;
	}

	if (dca < min)
		result = Rca;

	return (result);
}

bool CheckPointInTriangle(CVector point ,CVector p1, CVector p2, CVector p3)
{
	//point.z = p1.z = p2.z = p3.z = 0.f;

	CVector v1 = p1-point;
	CVector v2 = p2-point;
	CVector v3 = p3-point;

	v1.Normalize();
	v2.Normalize();
	v3.Normalize();

	float angle = acos(v1.DotProduct(v2));
	angle += acos(v2.DotProduct(v3));
	angle += acos(v3.DotProduct(v1));

	return (fabs(angle-2*PI) <= EPSILON);
}

float intersectRayPlane(CVector rayOrigin, CVector rayVector, CVector pOrigin, CVector pNormal)
{
	// The ray vector must be normalised
	float d = - (pNormal.DotProduct(pOrigin));
	float numer = pNormal.DotProduct(rayOrigin) + d; // This is the signed distance to the plane from the rayOrigin
	float denom = pNormal.DotProduct(rayVector);
  
	if (denom > 0)  // normal is orthogonal to vector, cant intersect
		return (-1.0f);
	return -(numer / denom);
}

float IntersectRaySphere(CVector p, CVector normalizedRay, CVector s, float radius)
{	
   CVector diff = s-p;
   // c is the distance to the sphere origin from the point
   float c = diff.Length();
   // v is the magnitude of the normalised ray in the direction of the sphere origin squared
   float v = diff.DotProduct(normalizedRay);
   float d = radius*radius - (c*c - v*v);

   // If there was no intersection, return -1
   if (d < 0.0) return (-1.0f);

   // Return the distance to the [first] intersecting point
   return (v - sqrt(d));
}

int ClassifyPoint(CVector point, CVector pN, CVector pO)
{
	CVector dir;
	float d;

	dir = pO - point;

	d = dir.DotProduct(pN);

	if (d<-0.001f)
		return 1;	// FRONT
	else
	if (d>0.001f)
		return 0;	//BACKSIDE

	return 1; // Hmmm...
}

SCollisionInfo SphereTriangleIntersection(CVector orig, CVector ray, float radius,
                   CVector vert0, CVector vert2, CVector vert1)
{
	SCollisionInfo colInfo;

	CVector v1 = vert1 - vert0;
	CVector v2 = vert2 - vert0;

	CVector pNormal = v1.CrossProduct(v2);
	pNormal.Normalize();

	CVector origin = orig, tRay, tVert0 = vert0, tVert1 = vert1, tVert2 = vert2;

	origin -= pNormal*radius;

	colInfo.bCollisionFound = false;

	// This will return what percentage of the ray's magnitude is the distance 
	// to the plane 1.0f is on the plane, 1.5 is 1.5 times the rays length
	float distanceToPlane = intersectRayPlane(origin, ray, vert0, pNormal);

	if ((distanceToPlane > 0.f)&&(distanceToPlane < 1.f))
	//if ((ClassifyPoint(orig, vert0, pNormal))&&(distanceToPlane < 1.f))
	{
		tRay = ray * distanceToPlane;
		origin += tRay;

		colInfo.bCollisionFound = CheckPointInTriangle(origin , tVert0, tVert1, tVert2);
		colInfo.planeDist = tRay.Length()-RAY_OFFSET;
		colInfo.planeNormal = pNormal;
	}

	if ((!colInfo.bCollisionFound)&&
		(ClassifyPoint(orig, pNormal, vert0))&&(!ClassifyPoint(orig-pNormal*radius, pNormal, vert0)))
	{
		tRay = ray * distanceToPlane;
		origin += tRay;

		colInfo.bCollisionFound = CheckPointInTriangle(origin , tVert0, tVert1, tVert2);
		colInfo.planeDist = tRay.Length()-RAY_OFFSET;
		colInfo.planeNormal = pNormal;
	}

	if (!colInfo.bCollisionFound)
	{
		float rayMagnitude = ray.Length();
		ray.Normalize();

		CVector normalizedVelocity = ray;

		CVector closestPoint = ClosestPointOnTriangle(orig, vert0, vert1, vert2);	

		float distToSphereIntersection = IntersectRaySphere(closestPoint, -normalizedVelocity, orig, radius);

		// we cannot know if the ray will actually hit the sphere so we need this check
		//if ((distToSphereIntersection > 0)&&(distToSphereIntersection < radius)) {
		if ((distToSphereIntersection > 0)&&(distToSphereIntersection < rayMagnitude)) {
			// calculate true sphere intersection point
			//CVector normal = orig-closestPoint;
			CVector normal = orig-(closestPoint+(-normalizedVelocity*distToSphereIntersection));
			normal.Normalize();
			colInfo.bCollisionFound = true;
			colInfo.collisionPoint = closestPoint;
			colInfo.planeNormal = normal;
			colInfo.planeDist = distToSphereIntersection-RAY_OFFSET;
		}
	}

	return colInfo;
}

colInfo.bCollisionFound = true;

CVector orig(objs->xForm.pos);
CVector velocity(objs->velocity);

while (colInfo.bCollisionFound)
{
	colInfo.bCollisionFound = false;

	level->CheckCollision(&orig, &velocity, colInfo);

	if (colInfo.bCollisionFound)
	{
		// Get the normalised velocity
		CVector normalisedVel = velocity;
		normalisedVel.Normalize();

		// The section of the velocity vector past the collision
		velocity = normalisedVel*(velocity.Length()-colInfo.planeDist);
		orig += normalisedVel*colInfo.planeDist;

		// Get the magnitude of the velocity vector 
		// along the negative normal that is past the collision point
		CVector velNormalMagnitude = -colInfo.planeNormal*(colInfo.planeNormal.DotProduct(velocity));

		// Adjust the player velocity vector by the magnitude of the velocity vector 
		// along the negative normal that is past the collision point
		objs->velocity += velNormalMagnitude;

		// Adjust the velocity vector by the magnitude of the velocity vector along 
		// the negative normal (This should now be perpendicular to the collision 
		// plane)
		velocity += velNormalMagnitude;
	}
}