#include "gpuCacheIsectUtil.h"
#include <MMatrix.h>
#include <MPointArray.h>
#include <limits>

namespace GPUCache {

	// used when ray does not intersect object
	// to account for perspective, all edges are flattened onto 
	// a plane defined by the raySource and rayDirection
	double gpuCacheIsectUtil::getClosestPointOnLine(const MPoint& queryPoint, const MPoint& pt1, const MPoint& pt2, MPoint& closestPoint){
		MVector edgeVec = pt2-pt1;
		double t = ( (queryPoint - pt1) * edgeVec ) / (edgeVec * edgeVec);
		if(t<0) t=0;
		if(t>1) t=1;
		closestPoint = (1-t) * pt1 + t * pt2;
		return t;
	}
	

	// used when ray does not intersect object
	// to account for perspective, all edges are flattened onto 
	// a plane defined by the raySource and rayDirection
	double gpuCacheIsectUtil::getEdgeSnapPointOnBox(const MPoint& raySource, const MVector& rayDirection, const MBoundingBox& bbox, MPoint& snapPoint){
		//if ray intersects bbox
		MPoint boxIntersectionPt;
		if(firstRayIntersection(bbox.min(), bbox.max(), raySource, rayDirection, NULL, &boxIntersectionPt))
		{
			//	ray intersects bounding box, so snapPoint is
			//	closest hit on the outside of the box
			//  and distance to box is 0 (for snapping purposes)
			snapPoint = boxIntersectionPt;
			return 0.0;
		}

		MPointArray verts;
		MPoint vmin = bbox.min();
		MPoint vmax = bbox.max();

		verts.insert(vmin,0);
		verts.insert(MPoint(vmax[0],vmin[1],vmin[2]),1);
		verts.insert(MPoint(vmax[0],vmax[1],vmin[2]),2);
		verts.insert(MPoint(vmin[0],vmax[1],vmin[2]),3);
		verts.insert(MPoint(vmin[0],vmin[1],vmax[2]),4);
		verts.insert(MPoint(vmax[0],vmin[1],vmax[2]),5);
		verts.insert(vmax,6);
		verts.insert(MPoint(vmin[0],vmax[1],vmax[2]),7);

		int edgeIndices[12][2]={{0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},{0,4},{1,5},{3,7},{2,6}};

		double minDistRect = std::numeric_limits<double>::max();
		
		for(int edge=0; edge<12;edge++){
			MPoint vertex1Org = verts[edgeIndices[edge][0]];
			MPoint vertex2Org = verts[edgeIndices[edge][1]];
			
			double coef_plane = rayDirection * raySource;
			double d = coef_plane - rayDirection * vertex1Org;
			MPoint vertex1 = vertex1Org + rayDirection * d;
			d = coef_plane - rayDirection * vertex2Org;
			MPoint vertex2 = vertex2Org + rayDirection * d;

			MVector edgeDir = vertex2 - vertex1;

			if (edgeDir.length()<0.0000001){
				double dist = vertex1.distanceTo(raySource);
				if (dist < minDistRect) {
					minDistRect = dist;
					snapPoint = vertex1Org;
				}
			} else {
				MPoint edgePt;
				// Compute the closest point from the edge to cursor Ray.
				double percent = gpuCacheIsectUtil::getClosestPointOnLine(raySource, vertex1, vertex2, edgePt);
				double dist = edgePt.distanceTo(raySource);
				if (dist < minDistRect) {
					minDistRect = dist;
					snapPoint =  (vertex1Org + percent * (vertex2Org - vertex1Org));
				}
			}
		}
		
		return minDistRect;
	}
	// used when ray does not intersect object
	// to account for perspective, all edges are flattened onto 
	// a plane defined by the raySource and rayDirection
	double gpuCacheIsectUtil::getEdgeSnapPointOnTriangle(const MPoint& raySource, const MVector& rayDirection, const MPoint& vert1, const MPoint& vert2, const MPoint& vert3, MPoint& snapPoint){
		MPointArray verts;
		verts.insert(vert1,0);
		verts.insert(vert2,1);
		verts.insert(vert3,2);

		int edgeIndices[3][2]={{0,1},{1,2},{2,0}};

		double minDistTri = std::numeric_limits<double>::max();

		for(int edge=0; edge<3;edge++){
			MPoint vertex1Org = verts[edgeIndices[edge][0]];
			MPoint vertex2Org = verts[edgeIndices[edge][1]];

			double coef_plane = rayDirection * raySource;
			double d = coef_plane - rayDirection * vertex1Org;
			MPoint vertex1 = vertex1Org + rayDirection * d;
			d = coef_plane - rayDirection * vertex2Org;
			MPoint vertex2 = vertex2Org + rayDirection * d;

			MVector edgeDir = vertex2 - vertex1;
			
			if (edgeDir.length()<0.0000001){
				double dist = vertex1.distanceTo(raySource);
				if (dist < minDistTri) {
					minDistTri = dist;
					snapPoint = vertex1Org;
				}
			} else {
				MPoint edgePt;
				// Compute the closest point from the edge to cursor Ray.
				double percent = gpuCacheIsectUtil::getClosestPointOnLine(raySource, vertex1, vertex2, edgePt);
				double dist = edgePt.distanceTo(raySource);                                                            
				if (dist < minDistTri) {
					minDistTri = dist;
					snapPoint =  (vertex1Org + percent * (vertex2Org - vertex1Org));
				}
			}
		}

		return minDistTri;
	}

	void gpuCacheIsectUtil::getClosestPointToRayOnLine(const MPoint& vertex1, const MPoint& vertex2, const MPoint& raySource, const MVector& rayDirection, MPoint& closestPoint, double& percent){
		MPoint clsPoint;
		MVector edgeDir = vertex2 - vertex1;
		double len = edgeDir.length();

		if(len < 0.0000001 )
		{
			percent = 0.0;
			closestPoint = vertex1;
			return;
		}

		edgeDir.normalize();
		
		//if line is parallel to ray
		double dotPrd = fabs(edgeDir * rayDirection);
		if(dotPrd > 0.9999){
			percent = 0.0;
			closestPoint = vertex1;
			return;
		}

		// Vector connecting two closest points.
		//
		MVector crossProd = edgeDir ^ rayDirection;

		// Normal to the plane defined by that vector and the 'otherLine'.
		//
		MVector planeNormal = rayDirection ^ crossProd;
		//intersectionPlane is raySource,planeNormal
		double t;
		if(intersectPlane(raySource, planeNormal, vertex1,edgeDir,t)){
			clsPoint = vertex1 + t * edgeDir;

			// Find percent, where
			// vertex1 + percent * (edgeDir) == closestPoint
			//
			percent = edgeDir * (clsPoint - vertex1) / len;

			// The closest point may not be on the segment. Find the closest
			// point on the segment using t.
			//
			if (percent < 0)
			{
				closestPoint = vertex1;
				percent = 0.0;
			}
			else if (percent > 1.0)
			{
				closestPoint = vertex2;
				percent = 1.0;
			}
			else
			{
				closestPoint = clsPoint;
			}
		} else
		{
			closestPoint = vertex1;
			percent = 0.0;
		}
	}

	double gpuCacheIsectUtil::getClosestPointOnBox(const MPoint& point, const MBoundingBox& bbox, MPoint& closestPoint){
		MVector diff = point - bbox.center();

		MVector axis[3] = { MVector(1.0,0.0,0.0),
			MVector(0.0,1.0,0.0),
			MVector(0.0,0.0,1.0)};

		double dimensions[3] = {0.5*bbox.width(),0.5*bbox.height(),0.5*bbox.depth()};

		double sqrDistance = 0.0;
		double delta;
		MPoint closest;

		for (int i = 0; i < 3; ++i)
		{
			closest[i] = diff * axis[i];
			if (closest[i] < -dimensions[i])
			{
				delta = closest[i] + dimensions[i];
				sqrDistance += delta*delta;
				closest[i] = -dimensions[i];
			}
			else if (closest[i] > dimensions[i])
			{
				delta = closest[i] - dimensions[i];  
				sqrDistance += delta*delta;
				closest[i] = dimensions[i];
			}
		}

		closestPoint = bbox.center();
		for (int i = 0; i < 3; ++i)
		{
			closestPoint += closest[i]*axis[i];
		}

		return sqrt(sqrDistance);
	}


	int gpuCacheIsectUtil::intersectRayWithBox(
		MPoint minPoint,
		MPoint maxPoint,
		const MPoint&	rayOrigin,
		const MVector&	rayDirection,
		double			isectParams[2]
	)
		//
		//	Description:
		//
		//		Utility function that finds parametric values of all
		//		intersections of a ray with the bounding box whose
		//		lower and upper bounds along each axis are defined
		//		by xBound, yBound, zBound.
		//
		//		Returns number of hits found (should always be <= 2),
		//		and returns parametric values of hits (sorted by increasing
		//		distance from the ray) through isectParams array.
		//		These parameters are the "t" values, if the ray is 
		//		expressed parametrically as P(t) = rayOrigin + t*rayDirection.
		//
	{
		//	small tolerance necessary when ray passes almost exactly through
		//	corners of the bounding box.
		//
		const double isectTol = 1.0e-6;

		//	how many hits have we found so far
		//
		int numFound = 0;

		//	put bounds in an array to let us index them by axis
		//
		double boundsMin[3]={minPoint[0],minPoint[1],minPoint[2]};
		double boundsMax[3]={maxPoint[0],maxPoint[1],maxPoint[2]};

		//	for each side of the voxel grid (+X, -X, +Y, -Y, +Z, -Z), we will
		//	intersect the ray with that side's plane, then check the intersection
		//	point to see if it lies within 
		//
		for( int axis = 0; axis < 3; axis++ )
		{
			//	ray can't intersect faces that it is parallel to 
			//
			if( rayDirection[axis] != 0 )
			{
				//	We are intersecting the ray with faces perp. to one axis 
				//	(X, Y, or Z).  Figure out what the other two axes are, as we're 
				//	going to have to test whether the intersect points are "inside"
				//	those faces.
				//
				int otherAxis1 = (axis+1)%3;
				int otherAxis2 = (axis+2)%3;

				//	figure out high and low "axis" coordinate values for the faces
				//	(x values of the -X and +X faces, for example)
				//			
				double sides[2] = { boundsMin[axis], boundsMax[axis] };

				//	find the ray intersection with the high and low faces for this
				//	axis, and determine if the hit points lie within the bounds
				//	for the other two axes.  For example, if the ray hits the plane
				//	defined by the +X face, does the hit point lie within the Y and Z
				//	ranges of the box.  If so, then the ray intersects the box at
				//	that point.
				//
				//	"side" is just 0 or 1: 0 for the low face (-X, for example),
				//	1 for the high face (1, for example).
				//
				for( int side = 0; side < 2; side++ )
				{
					//	find parametric distance to this face
					//	
					double tSide = (sides[side]-rayOrigin[axis])/rayDirection[axis];
					if( tSide > 0.0 )
					{
						//	find first other coordinate value of hit point (hit x
						//	axis, we figure out the y value, for example)
						//
						double newPointOtherAxis1 = rayOrigin[otherAxis1] + 
							tSide*rayDirection[otherAxis1];

						//	see if the bounding box for the first other axis 
						//	contains the hit point.  If not, the ray can't intersect
						//	this face of the bounding box
						//

						if( (newPointOtherAxis1 >= (boundsMin[otherAxis1]-isectTol)) && (newPointOtherAxis1 <= (boundsMax[otherAxis1]+isectTol)) )
						{
							//	now, test the hit point for the second other coordinate
							//	value, to see if it is inside the box bounds
							//
							double newPointOtherAxis2 = rayOrigin[otherAxis2] + 
								tSide*rayDirection[otherAxis2];


							if( (newPointOtherAxis2 >= (boundsMin[otherAxis2]-isectTol)) && (newPointOtherAxis2 <= (boundsMax[otherAxis2]+isectTol)) )
							{
								//	Point is on one face, inside the box bounds for
								//	the other two axes, so it's a hit.  
								//	Now, insert its parametric value into the hit
								//	param array, maintaining the array in
								//	sorted order of ascending t.  Note that since
								//	we are intersecting a ray against a convex 
								//	object, we should never have more than 2 
								//	intersections, so we'll just assume that the
								//	array currently has size 0 or 1, which makes
								//	the sorting trivial. 
								//
								//	The only time that this may not be the case
								//	is if a ray goes exactly through an edge or
								//	a corner.  In that case, all the intersections
								//	correspond to the same point, so we should only
								//	report it once.  We will achieve this by 
								//	discarding intersection param values that are
								//	"equivalent" (equal with a small numerical 
								//	tolerance). 
								//
								if( numFound == 0 )
								{
									isectParams[0] = tSide;
									numFound++;
								}
								else if( numFound == 1 )
								{
									//	add the hit param in the appropriate
									//	position in the array
									//
									if( tSide >= (isectParams[0]+1.0e-10) )
									{
										isectParams[1] = tSide;
										numFound++;
									}
									else if( tSide <= (isectParams[0]-1.0e-10) )
									{
										isectParams[1] = isectParams[0];
										isectParams[0] = tSide;
										numFound++;
									}
								}
							}
						}
					}
				}
			}
		}

		return numFound;
	}

	bool gpuCacheIsectUtil::firstRayIntersection ( 
		MPoint bboxMin,
		MPoint bboxMax,
		const MPoint&		rayOrigin,
		const MVector&		rayDirection,
		double*			isectParam,
		MPoint *		isectPoint
		) 
		//
		//	Description:
		//
		//		Finds first hit of ray against the outside of the bounding box.
		//		Returns true if the ray hits the box, false otherwise.
		//		"isectParam", if non-NULL, receives parametric distance along ray
		//		to the intersection point (ie the "t" value if the ray is expressed
		//		parametrically as P(t) = rayOrigin + t * rayDirection).
		//		"isectPoint", if non-NULL, receives the intersection point coordinates.
		//
	{
		//	get all the hits with the bounding box
		//
		double allIsectParams[4];

		if( !intersectRayWithBox( bboxMin, bboxMax, rayOrigin, rayDirection, allIsectParams ) )
		{
			return false;
		}
		else
		{
			//	Found hits, so closest one is first in array.  Return
			//	parametric value and point coordinates, if required.
			//
			if( isectParam != NULL )
			{
				*isectParam = allIsectParams[0];
			}

			if( isectPoint != NULL )
			{
				*isectPoint = rayOrigin + allIsectParams[0]*rayDirection;
			}

			return true;
		}
	}

	bool gpuCacheIsectUtil::intersectPlane(const MPoint &planePoint, const MVector &planeNormal, const MPoint& rayPoint, const MVector &rayDirection, double &t)
	{
		// assuming vectors are all normalized
		double denom = planeNormal * rayDirection;
		if (denom > 0.0000001) {
			MVector p0l0 = planePoint - rayPoint;
			t = (p0l0 * planeNormal) / denom; 
			return (t >= 0);
		}
		return false;
	}

	bool gpuCacheIsectUtil::getClosestPointOnTri(const MPoint &toThisPoint, const MPoint &pt1, const MPoint &pt2, const MPoint &pt3, MPoint &theClosestPoint, double &currDist) 
	{
		double		sum, a, b, c, len, dist;
		MMatrix mat;
		mat.setToIdentity();
		mat[2][0] = mat[2][1] = mat[2][2] = 1.;

		MVector v = toThisPoint - pt1;
		MVector v12 = pt2 - pt1;
		MVector v13 = pt3 - pt1;
		MVector norm = v12 ^ v13;
		len = norm * norm;
		if (len < 1.175494351e-38F) return false;
		len = ( norm * v ) / len;

		MPoint pnt = toThisPoint - len * norm;

		// Do a quick test first
		if (pnt.distanceTo(toThisPoint) >= currDist)
			return false;

		int i, j;				// Find best plane to project to
		if (fabs(norm[0]) > fabs(norm[1]))
		{
			if (fabs(norm[0]) > fabs(norm[2]))
			{
				i = 1; j = 2;
			}
			else
			{
				i = 0; j = 1;
			}
		}
		else
		{
			if (fabs(norm[1]) > fabs(norm[2]))
			{
				i = 0; j = 2;
				// i = 2; j = 0;
			}
			else
			{
				i = 0; j = 1;
			}
		}

		mat[0][0] = pt1[i]; mat[0][1] = pt2[i]; mat[0][2] = pt3[i]; 
		mat[1][0] = pt1[j]; mat[1][1] = pt2[j]; mat[1][2] = pt3[j]; 

		MMatrix	matInv = mat.inverse();
		MPoint abc(pnt[i], pnt[j], 1, 0);

		abc = matInv * abc;
		// Now abc is the barycentric coordinates of pnt
		// clip to inside triangle

		if (abc[0]<0) { // a < 0
			if (abc[1]<0) { // b < 0
				a = b = 0;
				c = 1;
			} else if (abc[2]<0) { // c < 0
				a = c = 0;
				b = 1;
			} else {
				a = 0;
				// c = BP dot BC / BC square;
				MVector v23 = pt3 - pt2; // BC
				MVector vp =  toThisPoint - pt2;  // BP

				c = ( vp * v23 ) / ( v23[0]*v23[0] + v23[1]*v23[1] + v23[2]*v23[2] );
				if (c<0) c = 0; else if (c>1) c = 1;
				b = 1 - c;
			}
		} else if (abc[1]<0) { // b < 0
			if (abc[2]<0) { // c < 0
				b = c = 0;
				a = 1;
				//} else if (abc[0]<0) { // a < 0
				//	b = a = 0;	// commented-code for optimization
				//	c = 1;		// leaving it in for readability (cyclic variations)
			} else {
				b = 0;
				// a = CP dot CA / CA square;
				MVector v31 = pt1 - pt3; // CA
				MVector vp =  toThisPoint - pt3;  // CP

				a = ( vp * v31 ) / ( v31[0]*v31[0] + v31[1]*v31[1] +v31[2]*v31[2] );
				if (a<0) a = 0; else if (a>1) a = 1;
				c = 1 - a;
			} 
		} else if (abc[2]<0) { // c < 0
			//if (abc[1]<0) { // b < 0
			//	c = b = 0;
			//	a = 1;
			//} else if (abc[0]<0) { // a < 0
			//	c = a = 0;
			//	b = 1;	// commented-code for optimization
			//} else {	// leaving it in for readability (cyclic variations)
			c = 0;
			// b = AP dot AB / AB square;
			//DIFF(v23, pt3, pt2); // AB
			MVector vp =  toThisPoint - pt1;  // AP

			b = ( vp * v12 ) / ( v12[0]*v12[0] + v12[1]*v12[1] + v12[2]*v12[2] );
			if (b<0) b = 0; else if (b>1) b = 1;
			a = 1 - b;
			//}
		} else {
			if (abc[0]>0) a = abc[0]; else a = 0;
			if (abc[1]>0) b = abc[1]; else b = 0;
			if (abc[2]>0) c = abc[2]; else c = 0;
		}
		sum = a+b+c;
		a /= sum ; b /= sum ; c /= sum ; 
		pnt = a * pt1 + b * pt2 + c * pt3;
		dist = pnt.distanceTo(toThisPoint);
		if ( dist < currDist)
		{			
			// Now it's really closer, keep it
			currDist = dist;
			theClosestPoint = pnt;
			return true;
		}
		return false;
	}
}