#include "collIntMat.h"

static Boolean setCollIntMatrixBE(Ulong, Ulong, CollIntMat *, int);

/*
 * Initialize a CollIntMat data structure
 *
 * Params	cim		pointer to CollIntMat data structure
 *
 * Globals	none
 *
 * Returns	nothing
 */
void initCollIntMatrix(cim)
CollIntMat *cim;
{
	cim->numObjs = 0;
	cim->rowStep = 1;
	cim->bitsPerLong = sizeof(Ulong) * 8;
	cim->matrix = (Ulong *)calloc(cim->bitsPerLong, sizeof(Ulong));
	if (cim->matrix == NULL) {
		fprintf(stderr, "Error: couldn't calloc storage for CIM\n");
		exit(1);
	}
}

/*
 * Given the row and column of the collision interest matrix, returns the
 * value at that location (1 or 0), which answers the question: does a
 * collision detection test have to be carried out between object a (row)
 * and object b (col).
 *
 * Params	row		the row (object a)
 *			col		the column (ojbect b)
 *			cim		collision interset matrix structure
 *
 * Globals	none
 *
 * Returns	1		collision detection has to be carried out
 *			0		collision detection doesn't need to be carried out
 *
 * Also have some undefined occurances -- checking beyond the currently known
 * number of objects, and checking an object against itself.
 */
Boolean queryCollIntMatrix(row, col, cim)
Ulong row, col;
CollIntMat *cim;
{
	Ulong max, rowIndex, colIndex, longOff, mask;

	if (row == col) {
	/* what should be done about object self testing ? */
		fprintf(stderr, "Object self intersection query\n");
		return True;
	}

	max = row > col ? row : col;

	if (max >= cim->numObjs) {
	/* what should be done about a query beyond scope of matrix ? */
		fprintf(stderr, "Warning: collision interest matrix queried beyond ");
		fprintf(stderr, "its scope.\n");
		return False;
	}

	rowIndex = row * cim->rowStep;
	colIndex = col / cim->bitsPerLong;
	longOff = col % cim->bitsPerLong;
	mask = 1 << longOff;

	return (cim->matrix[rowIndex + colIndex] & mask);
}

/*
 * Checks to see that the required cell of the matrix (the correct bit) fits 
 * into the current cim.  If it doesn't, increase the matrix.  Set the cell
 * to 'value' and update the number of objects (and other cim info).
 *
 * Params	row		which row
 *			col		which column
 *			cim		collision interest matrix
 *			value	1 or 0
 *
 * Globals	none
 *
 * Returns	True on successful completion with the cell set to value
 */
static Boolean setCollIntMatrixBE(row, col, cim, value)
Ulong row, col;
int value;
CollIntMat *cim;
{
	int bpl;
	Ulong rowIndex, colIndex, longOff;
	Ulong i, j, rs,no, reqLongs, max, mask, *temp;

	max = row > col ? row : col;
	reqLongs = (max / (bpl = cim->bitsPerLong)) + 1;

	if (reqLongs > cim->rowStep) {
		/* going to set a value beyond the memory range of current matrix
		 * so the matrix has to be grown
		 */
		temp = (Ulong *)calloc(reqLongs * reqLongs * bpl, sizeof(Ulong));
		if (temp == NULL) return False;

		/* copy old matrix into new matrix */
		for (i=0, no=cim->numObjs; i < no; i++)
			for (j=0, rs=cim->rowStep; j < rs; j++)
				temp[i*reqLongs+j] = cim->matrix[i*rs+j];
		
		/* set new values for the CollIntMat data structure */
		cim->rowStep = reqLongs;
		free(cim->matrix);
		cim->matrix = temp;
	}

	if (cim->numObjs <= max) cim->numObjs = max+1;

	/* at this point, the matrix cell in question exists so set its value */
	rowIndex = row * cim->rowStep;
	colIndex = col / bpl;
	longOff = col % bpl;
	mask = 1 << longOff;

	/* printf("r:%d c:%d l:%d\n", rowIndex, colIndex, longOff); */

	if (value) {
		cim->matrix[rowIndex + colIndex] |= mask;
	} else {
		mask = ~mask;
		cim->matrix[rowIndex + colIndex] &= mask;
	}
	return True;
}

/*
 * Outputs the collision interest matrix (because the numbers are unsigned
 * ints, can't rely on stopping loops with negative numbers.
 *
 * Params	cim		pointer to the collision interest matrix
 *
 * Globals	none
 *
 * Returns	nothings
 */
void printCollIntMatrix(cim)
CollIntMat *cim;
{
	int bpl;
	Ulong i, j, k, rs, rows;
	Ulong mask, current;
	
	rs = cim->rowStep;
	bpl = cim->bitsPerLong;

	for (i=0, rows=rs*bpl ; i<rows; i++) {
		for (j=rs-1; j>0; j--) {
			current = cim->matrix[i*rs+j];
			for (k=bpl-1; k>0; k--) {
				mask = 0x80000000 & current;
				current = current << 1;
				if (mask) mask = 1;
				printf("%u", mask);
			}
			mask = 0x80000000 & current;
			current = current << 1;
			if (mask) mask = 1;
			printf("%u", mask);
		}
		current = cim->matrix[i*rs+j];
		for (k=bpl-1; k>0; k--) {
			mask = 0x80000000 & current;
			current = current << 1;
			if (mask) mask = 1;
			printf("%u", mask);
		}
		mask = 0x80000000 & current;
		current = current << 1;
		if (mask) mask = 1;
		printf("%u", mask);

		printf("\n");
	}
}

/*
 * Given the ids of the objects, sets the corresponding cells to value, by
 * calling the backend function setCollIntMatrixBE.
 *
 * Params	u1		id of 1st object
 *			u2		id of 2nd object
 *			cim		pointer to the collision interest matrix
 *			value	1 or 0
 *
 * Globals	none
 *
 * Returns	True if there were no problems, False otherwise
 */
Boolean setCollIntMatrix(u1, u2, cim, value)
Ulong u1, u2;
int value;
CollIntMat *cim;
{
	if (!setCollIntMatrixBE(u1, u2, cim, value) ||
		!setCollIntMatrixBE(u2, u1, cim, value))
		return False;
	else
		return True;

}