#include "collDetect.h"
#include "testDB.h"
#include "polyPairTest.h"

extern Boolean verbose;

/*
 * Given a database of objects to test for intersection.  Each object has
 * a loc2glob specified, and also a pointer to a function which sets the
 * transform for any time t in the interval t=0 to t=1, as well as info to
 * pass to that function to help it change the transform.
 * Initially, the loc2glob transform is set for t=1.  Objects whose loc2glob
 * transform remain constant for 0 <= t <=1 have NULL pointer for the function
 * pointer ctrans, and NULL for the pointer info.
 *
 * Params	database		database of objects
 *			status			where to return the status of the database in
 *			time			where to return the time at which contact occurs,
 *							or the point where things are ok.
 *
 * Globals	verbose			say a lot?
 *
 * Returns	list of contact areas/points/lines if any (else NULL), and the
 *			status and time via the params.  The loc2glob transforms are valid
 *			also, so can be used to output the database at the time.
 */
ConNode *collisionDetect(database, status, time)
ObjDB database;
Status *status;
Real *time;
{
	ObjNode *obj;
	ConNode *cn = NULL;
	Real lower, upper, prevGuess, guess;

	/* test the status of the database at t=1.0 */
	cn = test4Intersection(database, status);

	if (*status == Nothing || *status == Contact) {
		/* no interpenetration */
		*time = 1.0;
		return cn;
	}

	/* there is intersection, so find the time at which there is only contact
	 * between objects, and no intersection -- use bisection.
	 */
	lower = 0.0; upper = 1.0;
	prevGuess = -1.0;

	if (verbose) fprintf(stderr, "*** First intersection of bisection\n"); 
	while (True) {
		guess = (lower + upper) / 2.0;
		if(verbose) fprintf(stderr, "guess: %.20f\n", guess);
		if (prevGuess == guess) {
			fprintf(stderr, "Error: couldn't find point of contact using");
			fprintf(stderr, " bisection\n");
			exit(2);
		}
		
		for (obj=database.objects; obj != NULL; obj=obj->next) {
			if (!(obj->valid)) continue;
			if (obj->ctrans != NULL) {
				(*(obj->ctrans))(guess, obj->info);
				calcBoundingBox(obj);
				if (obj->glob2loc != NULL) free(obj->glob2loc);
				obj->glob2loc = NULL;
			}
		}

		*status = Nothing;
		if (cn != NULL) destroyConNodeList(cn);
		cn = test4Intersection(database, status);

		if (*status == Nothing) {
			if (verbose) fprintf(stderr, "Status nothing\n");
			lower = guess;
		} else if (*status == Intersection) {
			if(verbose) fprintf(stderr, "Status intersection\n");
			upper = guess;
		} else {
			if(verbose) fprintf(stderr, "Status found contact\n");
			/* found time of contact */
			*time = guess;
			return cn;
		}

		prevGuess = guess;
	}
}