#include "seads.h"

static void voxelMembership(int, PolyNode *, SEADS *, Tupple *, Tupple);
static Boolean testVoxel(int, int, int, Tupple, Real *, Tupple);
static void addPoly2SEADS(int, int, SEADS *, Tupple, Tupple, FILE *);
static void fillGrid(char *, char, int, int);

/*
 * Given an object, creates the SEADS structure.  The object is in it's own
 * reference frame.  Also sets the max and min values (for the local ref
 * frame).  The object must lie in the positive octant ie. all co-ordinate 
 * values are >= 0.
 *
 * Params	obj		pointer to the object
 *
 * Globals	none
 *
 * Returns	nothing
 */
void createSEADS(obj)
ObjNode *obj;
{
	register int i, numVertices;
	int dimi, dimj, dimk;
	Tupple length, max, min;
	Tupple *v;

	dimi = 1; dimj = 1; dimk = 1;

	obj->seads = (SEADS *)malloc(sizeof(SEADS));
	if (obj->seads == NULL) {
		fprintf(stderr, "Error: Couldn't malloc SEADS node\n");
		exit(1);
	}

	for (i=0; i < ORDER; i++) {
		obj->seads->matrix[i].total = 0;
		obj->seads->matrix[i].a = NULL;
	}

	/* find the maximum and minimum bounds for each co-ord axis */
	/* Don't need to find the minimum (it's 0.0.0) but keep this code in
	 * case I remove the restriction on being in the positive octant
	 */
	max = obj->va.v[1];
	min = max;
	numVertices = obj->va.total;
	v = obj->va.v;
	for (i=2; i <= numVertices; i++) {
		if (v[i].i > max.i) max.i = v[i].i;
		if (v[i].i < min.i) min.i = v[i].i;
		if (v[i].j > max.j) max.j = v[i].j;
		if (v[i].j < min.j) min.j = v[i].j;
		if (v[i].k > max.k) max.k = v[i].k;
		if (v[i].k < min.k) min.k = v[i].k;
	}
	obj->max = max;
	obj->min = min;
	length.i = max.i - min.i;
	length.j = max.j - min.j;
	length.k = max.k - min.k;

	/* calculate the dimensions and voxel lengths of SEADS */
	for (i=0; i < M; i++) {
		if (length.i < length.j) {
			if (length.j < length.k) {
				length.k /= 2;
				dimk *= 2;
			} else {
				length.j /= 2;
				dimj *= 2;
			}
		} else {
			if (length.i < length.k) {
				length.k /= 2;
				dimk *= 2;
			} else {
				length.i /= 2;
				dimi *= 2;
			}
		}
	}

	obj->seads->dimi = dimi;
	obj->seads->dimj = dimj;
	obj->seads->dimk = dimk;
	obj->seads->voxSizes = length;

	/* translate object so that all points lie in the positive octant.
	 * The minimum co-ord value is 0 along any axis.
	 */
	for (i=1; i <= numVertices; i++) {
		v[i].i -= min.i;
		v[i].j -= min.j;
		v[i].k -= min.k;
	}

	/* go through all the polygons and determine which belong to which voxel */
	for (i=0; i < obj->polys.total; i++)
		voxelMembership(i, &(obj->polys.p[i]), obj->seads, obj->va.v, min);

	obj->max.i -= min.i;
	obj->max.j -= min.j;
	obj->max.k -= min.k;
	obj->min.i = 0.0;
	obj->min.j = 0.0;
	obj->min.k = 0.0;
}

/*
 * Given a polygon, the SEADS structure, and the vertices, adds the polygon's
 * index to the correct voxels in the SEADS structure.  It does this by
 * projecting the polygons onto 3 2D planes (x-y, y-z, z-x) and marking the
 * cells that belong to the projected areas.  Then each voxel is checked to
 * see that it is marked in all 3 planes, then the 8 corner points (translated
 * by appropriate amount) are tested against the plane equation of the polygon.
 *
 * Params:		pInd	index of the polygon
 * 				pp		pointer to the polygon in question
 *				s		pointer to the SEADS
 *				verts	pointer to an array containing the vertices
 *				trans	how far object has been moved
 *
 * Globals:		none
 *
 * Returns:		nothing
 */
static void voxelMembership(pInd, pp, s, verts, trans)
int pInd;
PolyNode *pp;
SEADS *s;
Tupple *verts, trans;
{
	register int i, j, k, xInd, yInd, zInd;
	Tupple point1, point2, voxSizes;
	Real plane[4], dx, dy, dz, tx, ty, tz, length, endt, t;
	int xInc, yInc, zInc, xBound, yBound, zBound, index, dimi, dimj, dimk;
	int *vertInd = (pp->vertices);
	char grid[ORDER];

	voxSizes = s->voxSizes;
	plane[0] = pp->normal.i;
	plane[1] = pp->normal.j;
	plane[2] = pp->normal.k;
	plane[3] = pp->d;
	dimi = s->dimi;
	dimj = s->dimj;
	dimk = s->dimk;

	for (i=0; i < ORDER; i++)
		grid[i] = 0;

	/* mark all the cells in the 2D grid that contain points or edges of
	 * the polygon
	 */

	/* go through all the edges */
	for (i=1; i <= pp->numVertices; i++) {

		/* set the starting and ending point of the current edge
		 * assert: point1 != point2
		 */
		point1 = verts[vertInd[i-1]];
		if (i == pp->numVertices)
			index = 0;
		else
			index = i;
		point2 = verts[vertInd[index]];

		/* work out gradients */
		dx = point2.i - point1.i;
		dy = point2.j - point1.j;
		dz = point2.k - point1.k;
		length = sqrt(dx * dx + dy * dy + dz * dz);
		dx /= length;
		dy /= length;
		dz /= length;

		/* determine end point in terms of parametric representation */
		if (dx != 0.0)
			endt = (point2.i - point1.i) / dx;
		else if (dy != 0.0)
			endt = (point2.j - point1.j) / dy;
		else
			endt = (point2.k - point1.k) / dz;

		/* set the index increments (+1 or -1) and which boundary of a 
		 * cell (the upper or lower, left or right) to test for exit from
		 * (dependent on sign of dx, dy, and dz)
		 */
		if (dx > 0.0) {
			xInc = 1; xBound = 1;
		} else {
			xInc = -1; xBound = 0;
		}
		if (dy > 0.0) {
			yInc = 1; yBound = 1;
		} else {
			yInc = -1; yBound = 0;
		}
		if (dz > 0.0) {
			zInc = 1; zBound = 1; 
		} else {
			zInc = -1; zBound = 0;
		}

		/* mark the grid cells containing the end point */
		if (voxSizes.i == 0.0)
			xInd = 0;
		else {
			xInd = (int) floor(point2.i / voxSizes.i);
			if (xInd == dimi) xInd--;
		}
		if (voxSizes.j == 0.0)
			yInd = 0;
		else {
			yInd = (int) floor(point2.j / voxSizes.j);
			if (yInd == dimj) yInd--;
		}
		if (voxSizes.k == 0.0)
			zInd = 0;
		else {
			zInd = (int) floor(point2.k / voxSizes.k);
			if (zInd == dimk) zInd--;
		}

		grid[xInd + yInd * dimi] |= 1;
		grid[yInd + zInd * dimj] |= 2;
		grid[zInd + xInd * dimk] |= 4;

		/* determine starting point indices */
		if (voxSizes.i == 0.0)
			xInd = 0;
		else {
			xInd = (int) floor(point1.i / voxSizes.i);
			if (xInd == dimi) xInd--;
		}
		if (voxSizes.j == 0.0)
			yInd = 0;
		else {
			yInd = (int) floor(point1.j / voxSizes.j);
			if (yInd == dimj) yInd--;
		}
		if (voxSizes.k == 0.0)
			zInd = 0;
		else {
			zInd = (int) floor(point1.k / voxSizes.k);
			if (zInd == dimk) zInd--;
		}

		/* beginning with the starting point of the line, find which voxels
		 * (and cells in 2D) the edge passes through and mark them, stopping 
		 * when we reach the end point
		 */
		t = 0.0;
		while (t < endt) {

			grid[xInd + yInd * dimi] |= 1;
			grid[yInd + zInd * dimj] |= 2;
			grid[zInd + xInd * dimk] |= 4;

			/* find which boundary (x, y, or z) the edge leaves the current
			 * voxel/cell through
			 */
			if (dx == 0.0)
				tx = HUGE_VAL;
			else
				tx = ((xInd + xBound) * voxSizes.i - point1.i) / dx;
			if (dy == 0.0)
				ty = HUGE_VAL;
			else
				ty = ((yInd + yBound) * voxSizes.j - point1.j) / dy;
			if (dz == 0.0)
				tz = HUGE_VAL;
			else
				tz = ((zInd + zBound) * voxSizes.k - point1.k) / dz;

			/* find the smallest t value, this is the boundary (or boundaries)
			 * the line cuts through first, so update indices to move into the
			 * next voxel.
			 */
			if (tx == ty && tx == tz) {
				xInd += xInc; yInd += yInc; zInd += zInc;
				t = tx;
			} else if (tx > ty && tx > tz) {
				if (ty == tz) {
					yInd += yInc; zInd += zInc; t = ty;
				} else if (ty < tz) {
					yInd += yInc; t = ty;
				} else {
					zInd += zInc; t = tz;
				}
			} else if (ty > tx && ty > tz) {
				if (tx == tz) {
					xInd += xInc; zInd += zInc; t = tx;
				} else if (tx < tz) {
					xInd += xInc; t = tx;
				} else {
					zInd += zInc; t = tz;
				}
			} else if (tz > tx && tz > ty) {
				if (tx == ty) {
					xInd += xInc; yInd += yInc; t = tx;
				} else if (tx < ty) {
					xInd += xInc; t = tx;
				} else {
					yInd += yInc; t = ty;
				}
			} else if (tx < ty) {
				xInd += xInc; t = tx;
			} else if (ty < tx) {
				yInd += yInc; t = ty;
			} else {
				zInd += zInc; t = tz;
			}
		}
	}

	/* have filled in all cells containing a vertex or edge, which forms a
	 * boundary for the polygon.  Next, mark all the enclosed cells.  The
	 * method assumes a convex polygon.
	 */

	fillGrid(grid, (char) 1, dimi, dimj);
	fillGrid(grid, (char) 2, dimj, dimk);
	fillGrid(grid, (char) 4, dimk, dimi);

	/* all the cells within the boundary of the polygon are marked, in the
	 * three planar projections (x-y, y-z, and z-x), so find which voxels
	 * have all corresponding cells marked, and also have the plane passing
	 * through.
	 */
	for (j=0; j < dimj; j++) {
		index = j * dimi;
		for (i=0; i < dimi; i++)
			if (grid[i + index] & (char) 1)
				for (k=0; k < dimk; k++)
					if ((grid[j+k*dimj]&(char)2) && (grid[k+i*dimk]&(char)4))
					/* the current voxel belongs "in" the areas defined by the
					 * 3 projections, so test if the plane passes through
					 */
					 	if (testVoxel(i, j, k, voxSizes, plane, trans)) {
							addPoly2SEADS(pInd, i + j*dimi + k*dimi*dimj, s);
						}
	}
}

/*
 * Given the indices of the voxel, the size of a voxel, the plane equation
 * of the polygon, the translation of the object, determines if the plane 
 * passes through the voxel
 *
 * Params		i, j, k		SEADS matrix indices of the voxel in question
 *								(allows calc of the 8 corner points)
 *				voxSizes	the dimensions of a voxel
 *				plane		contains the normal coeffs and 'd'
 *								ax + by + cz = d
 *				trans		translation of object from original position
 *
 * Globals		none
 *
 * Returns		True if plane crosses this voxel, otherwise False
 */
static Boolean testVoxel(i, j, k, voxSizes, plane, trans)
int i, j, k;
Tupple voxSizes, trans;
Real *plane;
{
	int x, y, z, sign;
	Real value;

	for (z=0; z<2; z++)
		for (y=0; y<2 ;y++)
			for(x=0; x<2; x++) {
				value = ((i + x) * voxSizes.i + trans.i) * plane[0] +
						((j + y) * voxSizes.j + trans.j) * plane[1] +
						((k + z) * voxSizes.k + trans.k) * plane[2] +
						plane[3];
				if (value != 0.0) value = value / fabs(value);
				if (x == 0 && y == 0 && z == 0)
					sign = (int) value;
				else
					if (sign != value)
						return True;
			}

	if (sign == 0) return True;

	return False;
}

/*
 * outputs the seads structure in ascii format
 */
void outputSEADS(obj)
ObjNode *obj;
{
	int i, j, k, q, di, dj, dk, index;
	SEADS *s = obj->seads;

	printf("ID: %u\n", obj->id);
	for (k=0, dk=obj->seads->dimk; k< dk; k++) {
		printf("K:%d\n", k);
		for (j=0, dj=obj->seads->dimj; j< dj; j++) {
			printf("J:%d\n", j);
			for (i=0, di=obj->seads->dimi; i< di; i++) {
				printf("(");
				index = i + j * di + k * di * dj;
				for (q=0; q < s->matrix[index].total; q++)
					printf("%d,", s->matrix[index].a[q]);
				printf(") ");
			}
			printf("\n");
		}
		printf("\n");
	}
}

/*
 * adds polygon index to correct SEADS voxel
 *
 * Params	pInd	the polygon's index
 *			index	which cell to add polygon to in the SEADS structure
 *			s		pointer to SEADS structure
 *
 * Globals	none
 *
 * Returns	nothing
 */
static void addPoly2SEADS(pInd, index, *s)
int pInd, index;
SEADS *s;
{
	register int i, limit;

	/* first check to make sure this polygon hasn't already been added */
	for (i=0, limit=s->matrix[index].total; i < limit; i++) 
		if (s->matrix[index].a[i] == pInd)
			return;

	(s->matrix[index].total)++;
	s->matrix[index].a = (int *)realloc(s->matrix[index].a,
							sizeof(int)*s->matrix[index].total);
	if (s->matrix[index].a == NULL) {
		fprintf(stderr, "Error: Couldn't realloc SEADS matrix\n");
		exit(1);
	}
	s->matrix[index].a[s->matrix[index].total-1] = pInd;

}	

/*
 * given the grid array/matrix, mask to use, and the dimensions, fill in the
 * cells that lie within the boundary of the area already marked in the grid.
 * The method assumes a convex area.  If not, a clustering algorithm can be used
 * or a 2nd pass going along the columns such that only cells marked by both
 * passes are accepted.
 *
 * Params	grid		pointer to grid
 *			mask		which bit we are working with
 *			dimx		dimension of nominal "x" axis
 *			dimy		dimension of nominal "y" axis
 *
 * Globals	none
 *
 * Returns	nothing
 */
static void fillGrid(grid, mask, dimx, dimy)
char *grid, mask;
int dimx, dimy;
{
	register int i, j, q;
	int left, right;
	Boolean found;

	for (j=0; j < dimy; j++) {
		/* find left most marked cell for current row */
		i = 0;
		found = True;
		q = j * dimx;
		while (!(grid[i + q] & mask)) {
			i++;
			if (i == dimx) {
				found = False;
				break;
			}
		}

		if (!found) continue;
		left = i;

		/* find right most marked cell for current row */
		i = dimx - 1;
		while (!(grid[i + q] & mask))
			i--;
		right = i;

		/* mark all cells between left and right */
		for (i=left+1; i < right; i++)
			grid[i + q] |= mask;
	}
}