#include "intersect.h"
#include "polyPairTest.h"
#include "database.h"
#include "matrix.h"

#ifdef DEBUG
#define	DBG(s)	s
#else
#define	DBG(s)
#endif

static Boolean HitBoundingBox(Tupple, Tupple, Ray, Tupple *);
static Boolean testRayObj(Ray, ObjNode *, Tupple *, Tupple *);
static Boolean testRayPolys(ObjNode *, Ray, int, Tupple *, Tupple *, char *,
				Real *);

#ifdef STATS
extern Ulong rayTestCount;
extern Ulong rayPolyTestCount;
extern Ulong rayVoxelCount;
#endif

/*
 * Will test if a ray (r) intersects any objects in the list pointed to by obj,
 * and returns the distance to intersection (measured in terms of the
 * ray's parametric value t), as well as the normal of the plane in the 
 * variable norm.
 *
 * The ray is first tested against the global bounding box.  If it intersects
 * it is transformed to the obj's reference frame and then the SEADS
 * structure of the object is made use of to reduce the number of polygons
 * tested against:  the ray is tracked from one voxel to the next, and tested 
 * with the polygons in the current voxel.  This continues until the ray
 * leaves the SEADS.
 *
 * Params	r		ray which consists of an originating point and a direction
 *					r(t) = origin + dir * t
 *			obj		pointer to list of objects to test for intersection with
 *			norm	variable in which to store the normal of the plane
 *					intersected with
 *
 * Globals	none
 *
 * Returns	distance to the intersection, and normal of the plane intersected
 *			with.
 */
Real intersect(r, obj, norm)
Ray r;
ObjNode *obj;
Tupple *norm;
{
	Real tMin = HUGE_VAL;
	Real t;
	Tupple gInterPnt, interPnt, ttemp, newNorm;
	Ray or;

	DBG(fprintf(stderr, "Entered intersect\n");)
	for (;obj != NULL; obj=obj->next) {

		/* test for intersection with global bounding box first */
		if (!HitBoundingBox(obj->gmin, obj->gmax, r, &gInterPnt))
			continue;

		/* find the t value, if greater than the tMin, don't bother testing */
		if (r.dir.i != 0.0) t = (gInterPnt.i - r.origin.i) / r.dir.i;
		else if (r.dir.j != 0.0) t = (gInterPnt.j - r.origin.j) / r.dir.j;
		else t = (gInterPnt.k - r.origin.k) / r.dir.k;

		if (t > tMin) continue;

		/* ray hits the global bounding box at least -- may not enter the 
		 * object's local reference frame
		 */

		/* transform ray to object's ref frame */
		if (obj->glob2loc == NULL) {
			obj->glob2loc = (Transform *)malloc(sizeof(Transform));
			if (obj->glob2loc == NULL) {
				fprintf(stderr, "Error: couldn't malloc Transform\n");
				exit(1);
			}
			if (!matrixInvert(obj->loc2glob, obj->glob2loc)) {
				fprintf(stderr, "Error: couldn't invert transform matrix\n");
				exit(2);
			}
		}

		applyTransform(obj->glob2loc, r.origin, &(or.origin));
		interPnt.i = r.origin.i + r.dir.i;
		interPnt.j = r.origin.j + r.dir.j;
		interPnt.k = r.origin.k + r.dir.k;
		applyTransform(obj->glob2loc, interPnt, &ttemp);
		or.dir.i = ttemp.i - or.origin.i;
		or.dir.j = ttemp.j - or.origin.j;
		or.dir.k = ttemp.k - or.origin.k;
		normalize(&(or.dir));

		/* test ray against bounding box in object's frame */
		if (!HitBoundingBox(obj->min, obj->max, or, &interPnt))
			continue;

		/* ray hits the SEADS so the ray has to be tracked through the
		 * structure. New ray origin = interPnt which lies in the structure
		 * unlike the origin of the ray which may or may not
		 */
		or.origin = interPnt;

		/* test ray against the object */
		if (testRayObj(or, obj, &interPnt, &newNorm)) {

			/* convert intersection point back to global ref frame, and
			 * find parametric value t
			 */
			applyTransform(obj->loc2glob, interPnt, &gInterPnt);
			if (r.dir.i != 0.0) t = (gInterPnt.i - r.origin.i) / r.dir.i;
			else if (r.dir.j != 0.0) t = (gInterPnt.j - r.origin.j) / r.dir.j;
			else t = (gInterPnt.k - r.origin.k) / r.dir.k;

			if (t < tMin && t > 0.0) {
				/* this point is closer than the best point found so far */
				tMin = t;

				/* new norm is in the local reference frame of the object, so
				 * find the global normal
				 */
				transformNormal(obj->glob2loc, newNorm, norm);
			}
		}
	}
	DBG(fprintf(stderr, "Left Intersect\n");)
	return tMin;
}

/*
 * Takes two points and determines if the line segment between the two,
 * intersects any of the objects in the object list
 *
 * Params	a, b	points
 *			obj		pointer to list of objects
 *
 * Globals	none
 *
 * Returns	True or False
 */
Boolean interfere(a, b, obj)
Tupple a, b;
ObjNode *obj;
{
	Real d, e, f, dist;
	Ray r;
	Tupple norm;

	DBG(fprintf(stderr, "Entered interfere\n");)
	r.origin = a;

	d = b.i - a.i;
	e = b.j - a.j;
	f = b.k - a.k;
	
	if ((dist = sqrt(d * d + e * e + f * f)) == 0.0) {
		fprintf(stderr, "Error magnitude of ray vector is 0\n");
		exit(2);
	}

	r.dir.i = d / dist;
	r.dir.j = e / dist;
	r.dir.k = f / dist;

	DBG(fprintf(stderr, "Left interfere\n");)
	if (intersect(r, obj, &norm) < dist)
		return True;
	else
		return False;
}

/*
 * given the min and max values defining a box, and a ray, determines whether
 * the ray intersects the box, and at which point it does.  Massaged version
 * of:
 *
 * ``Fast Ray-Box Intersection
 * by Andrew Woo
 * from "Graphics Gems", Academic Press, 1990''
 *
 * Params	min		minimums
 *			max		maximums
 *			r		ray
 *			pnt		where to return the hit point
 *
 * Globals	none
 *
 * Returns	True if the ray lies in the box or hits the box (and gives the
 *			point in coord), False otherwise.
 */

#define NUMDIM	3
#define RIGHT	0
#define LEFT	1
#define MIDDLE	2

static Boolean HitBoundingBox(min, max, r, pnt)
Tupple min, max, *pnt;
Ray r;
{
	Boolean inside = True;
	char quadrant[NUMDIM];
	register int i;
	int whichPlane;
	Real maxT[NUMDIM];
	Real candidatePlane[NUMDIM];
	Real minB[NUMDIM], maxB[NUMDIM], origin[NUMDIM], dir[NUMDIM];
	Real coord[NUMDIM];

	DBG(fprintf(stderr, "Entered HitBoundingBox\n");)
	minB[0] = min.i; minB[1] = min.j; minB[2] = min.k;
	maxB[0] = max.i; maxB[1] = max.j; maxB[2] = max.k;
	origin[0] = r.origin.i; origin[1] = r.origin.j; origin[2] = r.origin.k;
	dir[0] = r.dir.i; dir[1] = r.dir.j; dir[2] = r.dir.k;

	/* Find candidate planes; this loop can be avoided if
   	rays cast all from the eye(assume perpsective view) */
	for (i=0; i<NUMDIM; i++)
		if(origin[i] < minB[i]) {
			quadrant[i] = LEFT;
			candidatePlane[i] = minB[i];
			inside = False;
		}else if (origin[i] > maxB[i]) {
			quadrant[i] = RIGHT;
			candidatePlane[i] = maxB[i];
			inside = False;
		}else	{
			quadrant[i] = MIDDLE;
		}

	/* Ray origin inside bounding box */
	if(inside)	{
		pnt->i = origin[0]; pnt->j = origin[1]; pnt->k = origin[2];
		return True;
	}

	/* Calculate T distances to candidate planes */
	for (i = 0; i < NUMDIM; i++)
		if (quadrant[i] != MIDDLE && dir[i] !=0.)
			maxT[i] = (candidatePlane[i]-origin[i]) / dir[i];
		else
			maxT[i] = -1.;

	/* Get largest of the maxT's for final choice of intersection */
	whichPlane = 0;
	for (i = 1; i < NUMDIM; i++)
		if (maxT[whichPlane] < maxT[i])
			whichPlane = i;

	/* Check final candidate actually inside box */
	if (maxT[whichPlane] < 0.) return (False);
	for (i = 0; i < NUMDIM; i++)
		if (whichPlane != i) {
			coord[i] = origin[i] + maxT[whichPlane] *dir[i];
			if (coord[i] < minB[i] || coord[i] > maxB[i])
				return False;
		} else {
			coord[i] = candidatePlane[i];
		}

	pnt->i = coord[0]; pnt->j = coord[1]; pnt->k = coord[2];
	DBG(fprintf(stderr, "Left HitBoundingBox\n");)
	return True;				/* ray hits box */
}	

/*
 * given a ray (in the local reference frame of obj) tracks the ray through
 * the voxels, testing the ray against the polygons contained within the
 * current voxel.  Returns the closest intersection point (to the origin of
 * the ray), as well as the normal of the polygon it intersects (in the ref
 * frame of the object).
 *
 * Params	r		ray r(t) = origin + dir * t
 *			obj		object to test against
 *			inter	point of intersection (in obj ref frame) if any
 *			norm	normal of polygon involved in intersection, if any
 *
 * Globals	none
 *
 * Returns	True if an intersection occurs, False otherwise.  Also returns
 *			intersection point, and normal.
 */
static Boolean testRayObj(r, obj, inter, norm)
Ray r;
ObjNode *obj;
Tupple *inter, *norm;
{
	char *flag;
	SEADS *s = obj->seads;
	Real tMin = HUGE_VAL;
	Boolean hit = False;

	int xInd, yInd, zInd, xInc, yInc, zInc, dimi, dimj, dimk, dimidimj;
	int xAdd, yAdd, zAdd, index;
	Real tx, ty, tz, txInc, tyInc, tzInc;
	Tupple voxSizes;
	Boolean ok;

	#ifdef STATS
	rayTestCount++;
	#endif

	DBG(fprintf(stderr, "Entered testRayObj\n");)

	/* set up array to flag polygons that have already been tested */
	if ((flag = (char *)calloc(obj->polys.total, sizeof(char))) == NULL) {
		fprintf(stderr, "Error: couldn't calloc flag array\n");
		exit(1);
	}

	voxSizes.i = s->voxSizes.i; voxSizes.j = s->voxSizes.j;
	voxSizes.k = s->voxSizes.k;
	dimi = s->dimi; dimj = s->dimj; dimk = s->dimk;
	dimidimj = dimi * dimj;

	/* find starting voxel indices */
	if (voxSizes.i == 0.0) {
		xInd = 0;
	} else {
		xInd = (int) floor(r.origin.i / voxSizes.i);
		if (xInd >= dimi) xInd = dimi - 1;
	}
	if (voxSizes.j == 0.0) {
		yInd = 0;
	} else {
		yInd = (int) floor(r.origin.j / voxSizes.j);
		if (yInd >= dimj) yInd = dimj - 1;
	}
	if (voxSizes.k == 0.0) {
		zInd = 0;
	} else {
		zInd = (int) floor(r.origin.k / voxSizes.k);
		if (zInd >= dimk) zInd = dimk - 1;
	}

	/* setup the indices increments and work out the initial values for
	 * tx, ty, and tz which are the parametric values of the ray at which
	 * the ray leaves the current voxel through the x, y, and z planes.
	 */
	if (r.dir.i > 0.0) { xInc = 1; xAdd = 1; } else { xInc = -1; xAdd = 0; }
	if (r.dir.j > 0.0) { yInc = 1; yAdd = 1; } else { yInc = -1; yAdd = 0; }
	if (r.dir.k > 0.0) { zInc = 1; zAdd = 1; } else { zInc = -1; zAdd = 0; }

	if (r.dir.i == 0.0) {
		tx = HUGE_VAL;
	} else {
		tx = ((xInd + xAdd) * voxSizes.i - r.origin.i) / r.dir.i;
		txInc = voxSizes.i * xInc / r.dir.i;
	}
	if (r.dir.j == 0.0) {
		ty = HUGE_VAL;
	} else {
		ty = ((yInd + yAdd) * voxSizes.j - r.origin.j) / r.dir.j;
		tyInc = voxSizes.j * yInc / r.dir.j;
	}
	if (r.dir.k == 0.0) {
		tz = HUGE_VAL;
	} else {
		tz = ((zInd + zAdd) * voxSizes.k - r.origin.k) / r.dir.k;
		tzInc = voxSizes.k * zInc / r.dir.k;
	}

	while(ok) {
		index = xInd + yInd * dimi + zInd * dimidimj;

		#ifdef STATS
		rayVoxelCount++;
		#endif

		/* test polys in this voxel */
		if (testRayPolys(obj, r, index, inter, norm, flag, &tMin)) {
			hit = True;
		}
		
		/* move into next voxel.
		 * The smallest value of tx, ty, and tz indicates which boundary
		 * the ray leaves through
		 */
		if (tx == ty && tx == tz) {
			xInd += xInc; yInd += yInc; zInd += zInc;
			tx += txInc; ty += tyInc; tz += tzInc;
		} else if (tx > ty && tx > tz) {
			if (ty == tz) {
				yInd += yInc; zInd += zInc; ty += tyInc; tz += tzInc;
			} else if (ty < tz) {
				yInd += yInc; ty += tyInc;
			} else {
				zInd += zInc; tz += tzInc;
			}
		} else if (ty > tx && ty > tz) {
			if (tx == tz) {
				xInd += xInc; zInd += zInc; tx += txInc; tz += tzInc;
			} else if (tx < tz) {
				xInd += xInc; tx += txInc;
			} else {
				zInd += zInc; tz += tzInc;
			}
		} else if (tz > tx && tz > ty) {
			if (tx == ty) {
				xInd += xInc; yInd += yInc; tx += txInc; ty += tyInc;
			} else if (tx < ty) {
				xInd += xInc; tx += txInc;
			} else {
				yInd += yInc; ty += tyInc;
			}
		} else if (tx < ty) {
			xInd += xInc; tx += txInc;
		} else if (ty < tx) {
			yInd += yInc; ty += tyInc;
		} else {
			zInd += zInc; tz += tzInc;
		}

		if (xInd >= dimi || xInd < 0 || yInd >= dimj || yInd < 0 ||
			zInd >= dimk || zInd < 0) ok = False;
		else ok = True;
	}

	free (flag);
	return hit;
}

/*
 * given an object, the index of the voxel of the SEADS in question, and a ray
 * tests the ray against all the polygons in the voxel.  Returns the closest
 * intersection (if any), and the polygon involved, and True.  Otherwise False.
 * First checks whether the ray has been tested against the polygon or not, 
 * before finding the value of t.  Then checks that this value for t is less 
 * than the best value found so far (tMin), before finally checking to see that
 * the point of intersection is within the bounds of the polygon.
 *
 * Params	obj		object
 *			r		ray
 *			index	which voxel
 *			inter	return the point of intersection
 *			norm	return the normal of the polygon
 *			flag	array used to mark which polygons have already been
 *						tested.
 *			tMin	closest point of intersection so far, and also will be
 *						updated if a closer one is found in this voxel.
 *
 * Globals	none
 *
 * Returns	True for intersection, with the point in inter and the polygon's
 *			normal in norm, and the value of t (which is the closest point of
 *			intersection) else returns False.
 */
static Boolean testRayPolys(obj, r, index, inter, norm, flag, tMin)
ObjNode *obj;
Ray r;
int index;
Tupple *inter, *norm;
char *flag;
Real *tMin;
{
	Real denom, t;
	PolyNode *pp;
	int i, limit, *a;
	Tupple pnt;
	Boolean hit = False;

	DBG(fprintf(stderr, "Entered testRayPolys\n");)
	a = obj->seads->matrix[index].a;
	limit = obj->seads->matrix[index].total;

	for (i=0; i < limit; i++) {
		if (flag[a[i]]) {
			/* have already tested this polygon */
			continue;
		}

		#ifdef STATS
		rayPolyTestCount++;
		#endif

		flag[a[i]] = 1;
		pp = &(obj->polys.p[a[i]]);

		/* calculate point of intersection of ray and plane of polygon
		 * in terms of ray parameter t
		 */
		if ((denom = pp->normal.i * r.dir.i + pp->normal.j * r.dir.j +
			pp->normal.k * r.dir.k) == 0.0)
			continue;
			/* denominator is zero therefore ray and plane parallel */

		t = -(pp->d + pp->normal.i * r.origin.i + pp->normal.j * 
			r.origin.j + pp->normal.k * r.origin.k) / denom;

		/* if t is negative then the polygon is behind the ray
		 * if t is greater than the smallest t found so far 
		 * (corresponding to the closest found intersection) ignore it
		 *
		 * use ETA instead of 0.0 because of floating point errors
		 * less than this is considered t = 0.0 ie origin of ray
		 */
		if (t <= ETA || t >= *tMin) continue;

		/* ray intersects plane of polygon, so calc intersection point */
		pnt.i = r.origin.i + t * r.dir.i;
		pnt.j = r.origin.j + t * r.dir.j;
		pnt.k = r.origin.k + t * r.dir.k;

		/* test if point lies within the polygon's boundary */
		if (pointInPoly(pnt, pp, obj->va.v)) {
			hit = True;
			*tMin = t;
			*norm = pp->normal;
			*inter = pnt;
		}
	}

	DBG(fprintf(stderr, "Left testRayPolys\n");)
	return hit;
}