/***************************************************************************
 *            track.cc
 *
 *  Sat Nov 19 11:08:52 2005
 *  Copyright  2005  Joe Venzon
 *  joe@venzon.net
 ****************************************************************************/

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */
 
#include "track.h"

ROADSTRIP::ROADSTRIP()
{
	patchnodes = NULL;
}

void ROADSTRIP::ClearPatches()
{
	coltree.DeleteChildren();
	coltree.leaves.clear();
	
	while (patchnodes != NULL)
	{
		BEZIERNODE * oldfirst = patchnodes;
		patchnodes = patchnodes->next;
		delete oldfirst;
	}
}

BEZIER * ROADSTRIP::Add(BEZIER newpatch)
{
	BEZIERNODE * lastnode = NULL;
	BEZIERNODE * curnode = patchnodes;
	
	while (curnode != NULL)
	{
		lastnode = curnode;
		curnode = curnode->next;
	}

	//only continue if there is a last node and it really is the last node
	if (lastnode != NULL && lastnode->next == NULL)
	{
		lastnode->next = new BEZIERNODE;
		lastnode->next->patch.CopyFrom(newpatch);
		
		//optional....
		lastnode->patch.Attach(lastnode->next->patch);
		return &(lastnode->next->patch);
	}
	
	if (patchnodes == NULL)
	{
		patchnodes = new BEZIERNODE;
		patchnodes->patch.CopyFrom(newpatch);
		return &(patchnodes->patch);
	}
	
	return NULL;
}

BEZIER * ROADSTRIP::AddNew()
{
	BEZIER newpatch;
	return Add(newpatch);
}

bool ROADSTRIP::ReadFrom(ifstream &openfile)
{
	//optional....
	ClearPatches();
	
	lastcolpatch = NULL;
	
	if (!openfile)
		return false;
	
	BEZIER * newpatch = NULL;
	BEZIER temppatch;
	
	int num;
	
	//the number of patches for this road
	openfile >> num;
	
	int i;
	
	for (i = 0; i < num; i++)
	{
		//create a new patch and make it read from the file
		if (!temppatch.ReadFrom(openfile))
			return false;
		newpatch = Add(temppatch);
	}
	
	if (num > 2 && newpatch != NULL)
	{
		//close a looped track
		if (newpatch->points[0][0].equals(patchnodes->patch.points[3][0]) && newpatch->points[0][3].equals(patchnodes->patch.points[3][3]))
		{
			newpatch->Attach(patchnodes->patch);
		}
	}
	
	GenerateCollisionTree();
	
	return true;
}

bool ROADSTRIP::WriteTo(ofstream &openfile)
{
	BEZIERNODE * curnode = patchnodes;
	
	openfile << NumPatches() << endl << endl;
		
	while (curnode != NULL)
	{
		curnode->patch.WriteTo(openfile);
		openfile << endl;
		
		curnode = curnode->next;
	}
	
	return true;
}

int ROADSTRIP::NumPatches()
{
	BEZIERNODE * curnode = patchnodes;
	
	int num = 0;
	
	while (curnode != NULL)
	{
		num++;
		
		curnode = curnode->next;
	}
	
	return num;
}

bool ROADSTRIP::DeleteLastPatch()
{
	BEZIERNODE * lastnode = NULL;
	BEZIERNODE * prevlastnode = NULL;
	BEZIERNODE * curnode = patchnodes;
	
	while (curnode != NULL)
	{
		prevlastnode = lastnode;
		lastnode = curnode;
		curnode = curnode->next;
	}

	//only continue if there is a last node and it really is the last node
	if (lastnode != NULL && lastnode->next == NULL)
	{
		if (prevlastnode != NULL)
		{
			delete prevlastnode->next;
			prevlastnode->next = NULL;
			return true;
		}
		else
		{
			delete patchnodes;
			patchnodes = NULL;
			return true;
		}
	}
	
	return false;
}

BEZIER * ROADSTRIP::GetLastPatch()
{
	BEZIERNODE * lastnode = NULL;
	BEZIERNODE * prevlastnode = NULL;
	BEZIERNODE * curnode = patchnodes;
	
	while (curnode != NULL)
	{
		prevlastnode = lastnode;
		lastnode = curnode;
		curnode = curnode->next;
	}

	//only continue if there is a last node and it really is the last node
	if (lastnode != NULL && lastnode->next == NULL)
	{
		if (prevlastnode != NULL)
		{
			return &(prevlastnode->next->patch);
		}
		else
		{
			return &(patchnodes->patch);
		}
	}
	
	return NULL;
}

void ROADSTRIP::Visualize (bool wireframe, bool fill, VERTEX color)
{
	//int tot = 0;
	
	BEZIERNODE * curnode = patchnodes;
	
	/*while (curnode != NULL)
	{
		if (tot < 50) curnode->patch.Visualize(wireframe, fill, color);
		curnode = curnode->next;
		tot ++;
	}
	
	curnode = patchnodes;*/
	
	int count = 0;
	int drawn = 0;
	
	while (curnode != NULL)
	{
		//if (tot - count < 50)
		bool drawme = true;
		//VERTEX pos = curnode->patch.center+cam.position;
		VERTEX & pos = curnode->patch.center;
		for (int i=0;i<6;i++) 
		{
			float rd=cam.frustum[i][0]*pos.x+
			   cam.frustum[i][1]*pos.y+
			   cam.frustum[i][2]*pos.z+
			   cam.frustum[i][3];
			if (rd<=-curnode->patch.radius)
			{
				drawme = false;
			}
		}
		
		if (drawme)
		{
			curnode->patch.Visualize(wireframe, fill, color);
			drawn++;
		}
		
		curnode = curnode->next;
		count++;
	}
	
	//cout << drawn << "/" << count << endl;
}

bool ROADSTRIP::Collide(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest)
{
	BEZIER * cp;
	VERTEX normal;
	return Collide(origin, direction, outtri, closest, cp, normal);
}

bool ROADSTRIP::CollideBruteForce(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, BEZIER * & colpatch)
{
	BEZIERNODE * curnode = patchnodes;
	BEZIERNODE * lastnode = patchnodes;
	
	bool collide;
	bool hadcollision = false;
	VERTEX tvert;
	VERTEX tnorm;
	
	int count = 0;
	
	while (curnode != NULL)
	{
		//cout << "Patch " << count << endl;
		
		//curnode->patch.Visualize(wireframe, fill, color);
		collide = curnode->patch.BEZIER_COLLIDE_FUNCTION(origin, direction, tvert, tnorm);
		if (collide && !closest)
		{
			outtri = tvert;
			colpatch = &(curnode->patch);
			
			//normal = tnorm;
			
			//implicit caching
			lastnode->next = curnode->next;
			if (lastnode != curnode)
				curnode->next = patchnodes;
			patchnodes = curnode;
			
			return true;
		}
		else if (collide && closest)
		{
			if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
			{
				outtri = tvert;
				colpatch = &(curnode->patch);
			}
			
			hadcollision = true;
		}
		
		count++;
		
		lastnode = curnode;
		curnode = curnode->next;
	}
	
	if (closest && hadcollision)
	{
		return true;
	}
	
	return false;
}

bool ROADSTRIP::Collide(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, BEZIER * & colpatch, VERTEX & normal)
{
	bool col = false;
	
	/*if (lastcolpatch != NULL)
	{
		col = lastcolpatch->BEZIER_COLLIDE_FUNCTION(origin, direction, outtri);
		if (col && !closest)
		{
			colpatch = lastcolpatch;

			return true;
		}
	}*/
	
	col = CollideBranch(origin, direction, outtri, closest, colpatch, &coltree, normal);
	
	if (col)
		lastcolpatch = colpatch;
	
	return col;
}

bool ROADSTRIP::CollideBranch(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, BEZIER * & colpatch, BEZIERCOLBRANCH * branch, VERTEX & normal)
{
	if (branch == NULL)
		return false;
	
	bool verbose = false;
	
	if (verbose)
	{
		if (branch->left != NULL && branch->right != NULL)
		{
			cout << "Parent" << endl;
		}
		else
			cout << "Leaf" << endl;
	}
	
	//what to do if we're a leaf
	if (branch->left == NULL && branch->right == NULL)
	{
		bool collide;
		bool hadcollision = false;
		VERTEX tvert;
		VERTEX tnorm;
		
		int count = 0;
		
		list <BEZIERNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			//cout << "Patch " << count << endl;
			
			//curnode->patch.Visualize(wireframe, fill, color);
			collide = (*i1)->patch.BEZIER_COLLIDE_FUNCTION(origin, direction, tvert, tnorm);
			if (collide && !closest)
			{
				normal = tnorm;
				outtri = tvert;
				colpatch = &((*i1)->patch);
				
				//implicit caching
				/*lastnode->next = curnode->next;
				if (lastnode != curnode)
					curnode->next = patchnodes;
				patchnodes = curnode;*/
				
				return true;
			}
			else if (collide && closest)
			{
				if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
				{
					normal = tnorm;
					outtri = tvert;
					colpatch = &((*i1)->patch);
				}
				
				hadcollision = true;
			}
			
			count++;
		}
		
		if (closest && hadcollision)
		{
			return true;
		}
		
		return false;
	}
	
	if (verbose)
	{
		origin.DebugPrint();
		branch->bbox.DebugPrint();
	}
	
	//check bounding box
	if (!branch->bbox.IntersectRay(origin, direction))
		return false;
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	bool rcol = false;
	bool lcol = false;
	
	BEZIER * r_colpatch, * l_colpatch;
	VERTEX r_outtri, l_outtri;
	
	if (right != NULL)
		rcol = CollideBranch(origin, direction, r_outtri, closest, r_colpatch, branch->right, normal);
		
	//collision on the right branch
	/*if (rcol && !closest)
	{
		outtri = r_outtri;
		colpatch = r_colpatch;
		return rcol;
	}*/
	
	if (left != NULL)
		lcol = CollideBranch(origin, direction, l_outtri, closest, l_colpatch, branch->left, normal);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		outtri = l_outtri;
		colpatch = l_colpatch;
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		outtri = r_outtri;
		colpatch = r_colpatch;
		return rcol;
	}
	
	//collision on both branches
	if (lcol && rcol)
	{
		if ((origin - r_outtri).len() < (origin - l_outtri).len())
		{
			outtri = r_outtri;
			colpatch = r_colpatch;
			return rcol;
		}
		else
		{
			outtri = l_outtri;
			colpatch = l_colpatch;
			return lcol;
		}
	}
	
	return false;
}

void ROADSTRIP::GenerateCollisionTree()
{
	bool verbose = false;
	
	BEZIERNODE * curnode = patchnodes;
	
	if (verbose)
		cout << "Generating collision tree..." << endl;
	
	coltree.DeleteChildren();
	
	coltree.left = NULL;
	coltree.right = NULL;
	
	coltree.leaves.clear();
	
	while (curnode != NULL)
	{
		coltree.leaves.push_back(curnode);
		
		curnode = curnode->next;
	}
	
	GenerateBranches(&coltree);
	
	if (verbose)
		cout << "done" << endl;
}

void ROADSTRIP::GenerateBranches(BEZIERCOLBRANCH * branch)
{
	bool verbose = false;
	
	list <BEZIERNODE *>::iterator i1 = branch->leaves.begin();
	
	//build total aabb and find average vert center
	VERTEX avgcenter;
	float maxv[3];
	float minv[3];
	bool havevals[6];
	int n = 0;
	for (n = 0; n < 3; n++)
	{
		minv[n] = 0;
		maxv[n] = 0;
	}
	for (n = 0; n < 6; n++)
		havevals[n] = false;
	//maxvals.Set(-1e10,-1e10,-1e10);
	//minvals.Set(1e10,1e10,1e10);
	
	int numleaves = 0;
	
	for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		numleaves++;
	
	i1 = branch->leaves.begin();
	while (i1 != branch->leaves.end())
	{
		VERTEX p[4];
		p[0] = (*i1)->patch.points[0][0];
		p[1] = (*i1)->patch.points[0][3];
		p[2] = (*i1)->patch.points[3][3];
		p[3] = (*i1)->patch.points[3][0];
		
		int i, c;
		for (c = 0; c < 4; c++)
		{
			float * v = p[c].v3();
			for (i = 0; i < 3; i++)
			{
				if (v[i] > maxv[i] || !havevals[i])
				{
					maxv[i] = v[i];
					havevals[i] = true;
				}
				if (v[i] < minv[i] || !havevals[i+3])
				{
					minv[i] = v[i];
					havevals[i+3] = true;
				}
			}
		}
		
		VERTEX pcenter = p[0] + p[1] + p[2] + p[3];
		pcenter.Scale(0.25);
		avgcenter = avgcenter + pcenter.ScaleR(1.0/numleaves);
		
		i1++;
	}
	
	VERTEX minvals, maxvals;
	minvals.Set(minv);
	maxvals.Set(maxv);
	branch->bbox.SetFromCorners(minvals, maxvals);
	if (verbose)
	{
		cout << "Bounding box:" << endl;
		branch->bbox.GetPos().DebugPrint();
		branch->bbox.GetSize().DebugPrint();
		branch->bbox.GetCenter().DebugPrint();
	}
	
	//find axis of maximum change
	VERTEX axismask;
	axismask.Set(1,0,0);
	if (maxvals.x - minvals.x > maxvals.y - minvals.y && maxvals.x - minvals.x > maxvals.z - minvals.z)
	{
		axismask.Set(1,0,0);
	}
	else if (maxvals.y - minvals.y > maxvals.x - minvals.x && maxvals.y - minvals.y > maxvals.z - minvals.z)
	{
		axismask.Set(0,1,0);
	}
	else if (maxvals.z - minvals.z > maxvals.y - minvals.y && maxvals.z - minvals.z > maxvals.x - minvals.x)
	{
		axismask.Set(0,0,1);
	}
	
	int ll = 0;
	int lr = 0;
	
	//only propagate leaves if we're not a leaf
	if (numleaves > LEAVES_PER_BBOX)
	{
		//create children
		branch->left = new BEZIERCOLBRANCH;
		branch->right = new BEZIERCOLBRANCH;
		
		int distributor = 0; //hack to stop infinite recursion
		
		//throw leaves into children
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			VERTEX p[4];
			p[0] = (*i1)->patch.points[0][0];
			p[1] = (*i1)->patch.points[0][3];
			p[2] = (*i1)->patch.points[3][3];
			p[3] = (*i1)->patch.points[3][0];
			
			VERTEX pcenter = p[0] + p[1] + p[2] + p[3];
			pcenter.Scale(0.25);
			
			if (pcenter.dot(axismask) - avgcenter.dot(axismask) > 0.0)
				branch->right->leaves.push_back(*i1);
			else if (pcenter.dot(axismask) - avgcenter.dot(axismask) < 0.0)
				branch->left->leaves.push_back(*i1);
			else
			{
				//leaf is right at average, distribute evenly
				if (distributor % 2 == 0)
					branch->right->leaves.push_back(*i1);
				else
					branch->left->leaves.push_back(*i1);
				
				distributor++;
			}
		}
		
		//clear out leaves
		branch->leaves.clear();
		
		//count leaves of children
		for (i1 = branch->left->leaves.begin(); i1 != branch->left->leaves.end(); i1++)
			ll++;
		for (i1 = branch->right->leaves.begin(); i1 != branch->right->leaves.end(); i1++)
			lr++;
		
		if (verbose) cout << "Parent Leaves: " << numleaves << " L: " << ll << " R: " << lr << endl;
		
		if (ll == 0 || lr == 0)
		{
			if (lr != 0)
			{
				for (i1 = branch->right->leaves.begin(); i1 != branch->right->leaves.end(); i1++)
					branch->leaves.push_back(*i1);
				branch->right->leaves.clear();
			}
			
			if (ll != 0)
			{
				for (i1 = branch->left->leaves.begin(); i1 != branch->left->leaves.end(); i1++)
					branch->leaves.push_back(*i1);
				branch->left->leaves.clear();
			}
			
			delete branch->left;
			branch->left = NULL;
			
			delete branch->right;
			branch->right = NULL;
		}
		else
		{
			GenerateBranches(branch->left);
			GenerateBranches(branch->right);
		}
		
		/*if (ll == 0)
		{
			delete branch->left;
			branch->left = NULL;
		}
		else GenerateBranches(branch->left);
		
		if (lr == 0)
		{
			delete branch->right;
			branch->right = NULL;
		}
		else GenerateBranches(branch->right);*/
	}
	else
	{
		branch->right = NULL;
		branch->left = NULL;
		
		if (verbose) cout << "(Leaf)" << endl;
	}
}


//----------------- TRACK --------------


TRACK::TRACK()
{
	roads = NULL;
	lastelev = -100;
}

TRACK::~TRACK()
{
	ClearRoads();
}

void TRACK::ClearRoads()
{
	while (roads != NULL)
	{
		ROADSTRIPNODE * oldfirst = roads;
		roads = roads->next;
		delete oldfirst;
	}
}

ROADSTRIP * TRACK::AddNewRoad()
{
	/*ROADSTRIPNODE * oldfirst = roads;
	roads = new ROADSTRIPNODE;
	roads->next = oldfirst;
	
	return &(roads->road);*/
	
	ROADSTRIPNODE * lastnode = NULL;
	ROADSTRIPNODE * curnode = roads;
	
	while (curnode != NULL)
	{
		lastnode = curnode;
		curnode = curnode->next;
	}

	//only continue if there is a last node and it really is the last node
	if (lastnode != NULL && lastnode->next == NULL)
	{
		lastnode->next = new ROADSTRIPNODE;
		//lastnode->next->patch.CopyFrom(newpatch);
		
		//optional....
		//lastnode->patch.Attach(lastnode->next->patch);
		return &(lastnode->next->road);
	}
	
	if (roads == NULL)
	{
		roads = new ROADSTRIPNODE;
		return &(roads->road);
	}
	
	return NULL;
}

void TRACK::VisualizeRoads(bool wireframe, bool fill, ROADSTRIP * selectedroad)
{
	ROADSTRIPNODE * curnode = roads;
	
	VERTEX color;
	color.zero();
	color.z = 1;
	
	//utility.seedrandom(1234);
	
	while (curnode != NULL)
	{
		/*//randomize color!
		color.x = utility.randf(0.0,1.0);
		color.y = utility.randf(0.0,1.0);
		color.z = utility.randf(0.0,1.0);
		
		if (color.len() < 0.1)
		{
			color.y = 1.0;
		}
		
		float maxval = 0.0;
		if (color.x > maxval)
			maxval = color.x;
		if (color.y > maxval)
			maxval = color.y;
		if (color.z > maxval)
			maxval = color.z;
		
		color.x = (1.0/maxval)*color.x;
		color.y = (1.0/maxval)*color.y;
		color.z = (1.0/maxval)*color.z;*/
		
		color.zero();
		if (&(curnode->road) == selectedroad)
			color.y = 1;
		else
		{
			color.y = 0.5;
			color.z = 0.5;
		}
		
		curnode->road.Visualize(wireframe, fill, color);
		curnode = curnode->next;
	}
}

void TRACK::Write(string trackname)
{
	ofstream trackfile;
	trackfile.open((settings.GetDataDir() + "/tracks/" + trackname + "/roads.trk").c_str());
	
	VERTEX sl = GetStart();
	trackfile << sl.x << " " << sl.y << " " << sl.z << endl << endl;
	
	trackfile << NumRoads() << endl << endl;
	
	ROADSTRIPNODE * curnode = roads;
	
	while (curnode != NULL && trackfile)
	{
		curnode->road.WriteTo(trackfile);
		curnode = curnode->next;
	}
}

int TRACK::NumRoads()
{
	ROADSTRIPNODE * curnode = roads;
	
	int num = 0;
	
	while (curnode != NULL)
	{
		num++;
		curnode = curnode->next;
	}
	
	return num;
}

void TRACK::Load(string trackname)
{	
	ifstream trackfile;
	trackfile.open((settings.GetDataDir() + "/tracks/" + trackname + "/roads.trk").c_str());
	
	CONFIGFILE trackconfig;
	trackconfig.Load((settings.GetDataDir() + "/tracks/" + trackname + "/track.txt").c_str());
	float tvert[3];
	VERTEX sl;
	trackconfig.GetParam("start position", tvert);
	string strcullfaces;
	trackconfig.GetParam("cull faces", strcullfaces);
	sl.Set(tvert);
	
	if (strcullfaces == "no")
		cullfaces = false;
	else
		cullfaces = true;
	
	int numroads, i;
	
	/*trackfile >> sl.x;
	trackfile >> sl.y;
	trackfile >> sl.z;*/
	
	SetStart(sl);
	
	VERTEX sov;
	trackconfig.GetParam("start orientation-xyz", tvert);
	sov.Set(tvert);
	QUATERNION so;
	so.x = sov.x;
	so.y = sov.y;
	so.z = sov.z;
	trackconfig.GetParam("start orientation-w", so.w);
	SetStartOrientation(so);
	
	if (trackfile)
	{
		trackfile >> numroads;
	}
	else
	{
		cout << "No track named " << trackname << ", creating a new one." << endl;
		return;
	}
	
	ClearRoads();
	
	ROADSTRIP * newroad;
	
	for (i = 0; i < numroads && trackfile; i++)
	{
		newroad = AddNewRoad();
		newroad->ReadFrom(trackfile);
	}
	
	lapsequence.clear();
	int lapmarkers;
	if (trackconfig.GetParam("lap sequences", lapmarkers))
	{
		for (int l = 0; l < lapmarkers; l++)
		{
			float lapraw[3];
			stringstream lapname;
			lapname << "lap sequence " << l;
			trackconfig.GetParam(lapname.str(), lapraw);
			lapsequence.push_back(GetBezier((int) lapraw[0], (int) lapraw[1]));
		}
	}
	
	float f;
	trackconfig.GetParam("non-treaded friction coefficient", f);
	friction1 = f;
	trackconfig.GetParam("treaded friction coefficient", f);
	friction2 = f;
}

void TRACK::Delete(ROADSTRIP * striptodel)
{
	bool deleted = false;
	
	ROADSTRIPNODE * lastnode = NULL;
	ROADSTRIPNODE * curnode = roads;
	
	while (curnode != NULL)
	{
		/*
		NOTE THAT THE BACKSPACE BUTTON NEEDS TO CALL THIS (AND CLEAR 
		 ACTIVESTRIP TO NULL) TO ENSURE THERE ARE NO EMPTY ROADS!
		*/
		
		if (striptodel == &(curnode->road))
		{
			curnode = curnode->next;

			if (lastnode == NULL)
			{
				roads = curnode;
			}
			else
			{
				lastnode->next = curnode;
			}
			
			delete striptodel;
			
			deleted = true;
		}
		else
		{
			lastnode = curnode;
			curnode = curnode->next;
		}
	}
	
	//fail safe stuff
	if (!deleted)
		delete striptodel;
	
	striptodel = NULL;
}

bool TRACK::CollideRoads(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, ROADSTRIP * &collideroad, BEZIER * & collidepatch, VERTEX & normal)
{
	ROADSTRIPNODE * curnode = roads;
	
	bool collide;
	bool hadcollision = false;
	VERTEX tvert;
	VERTEX tnorm;
	
	while (curnode != NULL)
	{
		//curnode->patch.Visualize(wireframe, fill, color);
		collide = curnode->road.Collide(origin, direction, tvert, closest, collidepatch, tnorm);
		if (collide && !closest)
		{
			outtri = tvert;
			collideroad = &(curnode->road);
			normal = tnorm;
			return true;
		}
		else if (collide && closest)
		{
			if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
			{
				normal = tnorm;
				outtri = tvert;
				collideroad = &(curnode->road);
			}
			
			hadcollision = true;
		}
		
		curnode = curnode->next;
	}
	
	if (closest && hadcollision)
	{
		return true;
	}
	
	outtri = origin;
	return false;
}

bool TRACK::Collide(VERTEX origin, VERTEX direction, float seglen, VERTEX &outtri, bool closest, VERTEX & normal, float & dist)
{
	bool verbose = false;
	
	bool col = false;

	dist = 0;
	
	VERTEX zero;
	
	if (direction.equals(zero))
		return false;
	
	direction = direction.normalize();
	
	//ROADSTRIP * colstrip;
	//BEZIER * colpatch;
	
	normal.Set(0,1,0);
	
	/*col = CollideRoads(origin, direction, outtri, closest, colstrip, colpatch, normal);
	
	if (col)
	{
		if (verbose)
			cout << "track collision" << endl;
	}
	else
	{*/
		//check for a model collision
		OBJECTNODE * tnode;
		col = objects.Collide(origin, direction, outtri, closest, normal, seglen, tnode);
		//normal.Set(0,1,0);
		//normal.DebugPrint();
		if (col)
		{
			dist = (outtri - origin).len();
			if (dist > seglen)
				return false;
			if (verbose)
			{
				cout << "model collision: " << dist << endl;
				//lastorigin.DebugPrint();
				outtri.DebugPrint();
				cout << endl;
			}
		}
	//}
	
	//if (!col && verbose) cout << "no collision" << endl;

	//normal.DebugPrint();
	
	if (normal.nan())
	{
		cout << "Detected NaN in normal vector" << endl;
		normal.Set(0,1,0);
	}
	
	if (outtri.nan())
	{
		cout << "Detected NaN in collision point" << endl;
		outtri = origin;
	}
	
	if (!(dist <= 0 || dist > 0))
	{
		cout << "Detected NaN in collision distance" << endl;
		dist = 0;
	}
	
	return col;
}

bool TRACK::CollideD(VERTEXD origin, VERTEXD direction, double seglen, VERTEXD &outtri, bool closest, VERTEXD & normal, double & dist)
{
	bool verbose = false;
	
	bool col = false;

	dist = 0;
	
	if (direction.x == 0 && direction.y == 0 && direction.z == 0)
		return false;
	
	direction = direction.normalize();
	
	//ROADSTRIP * colstrip;
	//BEZIER * colpatch;
	
	normal.Set(0,1,0);
	

	//check for a model collision
	col = objects.CollideD(origin, direction, outtri, closest, normal, seglen);
	//normal.Set(0,1,0);
	//normal.DebugPrint();
	if (col)
	{
		dist = (outtri - origin).len();
		//if (dist > seglen || (outtri-origin).dot(direction) >= 0)
		/*if (dist > seglen)
		{
			//cout << "weird dist length (track.cpp)" << endl;
			return false;
		}*/
		if (verbose)
		{
			cout << "model collision: " << dist << endl;
			//lastorigin.DebugPrint();
			outtri.DebugPrint();
			cout << endl;
		}
	}
	
	//if (!col && verbose) cout << "no collision" << endl;

	//normal.DebugPrint();
	
	if (normal.nan())
	{
		cout << "Detected NaN in normal vector" << endl;
		normal.Set(0,1,0);
	}
	
	if (outtri.nan())
	{
		cout << "Detected NaN in collision point" << endl;
		outtri = origin;
	}
	
	if (!(dist <= 0 || dist > 0))
	{
		cout << "Detected NaN in collision distance" << endl;
		dist = 0;
	}
	
	return col;
}

double TRACK::Elevation(VERTEX origin)
{
	VERTEX normal;
	return Elevation(origin, normal);
}

double TRACK::Elevation(VERTEX origin, VERTEX & normal)
{
	origin.y += 1000;
	return ElevationSeg(origin, normal, 10000.0f);
}

double TRACK::ElevationSeg(VERTEX origin, VERTEX & normal, float seglen)
{
	BEZIER * colpatch;
	OBJECTNODE * colnode;
	return ElevationSeg(origin, normal, seglen, colpatch, colnode);
}

double TRACK::ElevationSeg(VERTEX origin, VERTEX & normal, float seglen, BEZIER * &colpatch, OBJECTNODE * &colnode)
{
	if (origin.nan())
	{
		cout << "Detected NaN in origin vector" << endl;
	}
	
	if (!(seglen < 0 || seglen >= 0))
	{
		cout << "Detected NaN in seglen" << endl;
	}
	
	bool verbose = false;
	
	//origin.y = 1000;
	VERTEX dir;
	dir.y = -1;
	double elev = -100000;
	VERTEX colpt;
	bool col = false;
	bool bezcol = false;
	bool modelcol = false;
	
	ROADSTRIP * colstrip;
	//BEZIER * colpatch;
	
	normal.Set(0,1,0);
	
	//bool closest = false;
	bool closest = true;
	
	colnode = NULL;
	colpatch = NULL;
	
	col = CollideRoads(origin, dir, colpt, closest, colstrip, colpatch, normal);
	
	//if (Collide(origin, dir, colpt, true))
	//if (0)
	if (col)
	{
		bezcol = true;
		elev = colpt.y;
		if (verbose)
			cout << "track collision" << endl;
		/*colpatch->points[0][0].DebugPrint();
		colpatch->points[0][3].DebugPrint();
		colpatch->points[3][0].DebugPrint();
		colpatch->points[3][3].DebugPrint();*/
	}
	else
	{
		if (objects.GetCollideAndDriveTogether())
		{
			colpatch = NULL;
			
			closest = true;
			col = objects.Collide(origin, dir, colpt, closest, normal, seglen, colnode);
			if (col)
			{
				elev = colpt.y;
				modelcol = true;
			}
			else
				elev = origin.y - seglen;
		}
		else
		{
			//check for a model collision
			closest = false;
			col = objects.CollideDriveable(origin, dir, colpt, closest, normal);
			//normal.Set(0,1,0);
			//normal.DebugPrint();
			if (col)
			{
				elev = colpt.y;
				if (verbose)
					cout << "model collision" << endl;
			}
		}
	}
	
	if (!col && verbose)
		cout << "no collision" << endl;
	
	//normal.DebugPrint();
	
	if (col)
	{
		if (normal.nan())
		{
			cout << "Detected NaN in normal vector " << bezcol << " " << modelcol << endl;
			normal.Set(0,1,0);
		}
		
		if (!(elev < 0 || elev >= 0))
		{
			cout << "Detected NaN in elevation " << bezcol << " " << modelcol << endl;
			/*cout << "bezcol: " << bezcol << endl;
			cout << "modelcol: " << modelcol << endl;
			origin.DebugPrint();
			dir.DebugPrint();*/
			return -100000;
		}
	}
	
	return elev;
}

BEZIERCOLBRANCH::BEZIERCOLBRANCH()
{
	left = NULL;
	right = NULL;
	leaves.clear();
}

void BEZIERCOLBRANCH::DeleteChildren()
{
	leaves.clear();
	
	if (left != NULL)
	{
		left->DeleteChildren();
		delete left;
		left = NULL;
	}
	
	if (right != NULL)
	{
		right->DeleteChildren();
		delete right;
		right = NULL;
	}
}

bool TRACK::CollideModel(VERTEX * modelverts, int numfaces, AABB bbox, VERTEX & outtri, bool closest, VERTEX & normal, float & depth)
{
	return objects.CollideModel(modelverts, numfaces, bbox, outtri, closest, normal, depth);
}

BEZIER * TRACK::GetPatch(VERTEX corners[4])
{
	ROADSTRIPNODE * curnode = roads;
	
	while (curnode != NULL)
	{
		BEZIERNODE * curpatch = curnode->road.GetFirstNode();
		while (curpatch != NULL)
		{
			if (curpatch->patch.points[0][0].equals(corners[0]) && 
				curpatch->patch.points[0][3].equals(corners[1]) && 
				curpatch->patch.points[3][0].equals(corners[2]) && 
				curpatch->patch.points[3][3].equals(corners[3]))
			{
				return &(curpatch->patch);
			}
			
			curpatch = curpatch->next;
		}
		
		curnode = curnode->next;
	}
	
	return NULL;
}

BEZIER * TRACK::GetBezier(int ridx, int pidx)
{
	ROADSTRIPNODE * curnode = roads;
	
	int ri = 0;
	
	while (curnode != NULL)
	{
		if (ri == ridx)
		{
			BEZIERNODE * curp = curnode->road.patchnodes;
			int pi = 0;
			
			while (curp != NULL)
			{
				if (pidx == pi)
				{
					return &(curp->patch);
				}
				
				pi++;
				curp = curp->next;
			}
		}
		
		ri++;
		curnode = curnode->next;
	}
	
	cerr << "Error finding bezier for lap sequence in GetLapSeq(" << ridx << "," << pidx << ")" << endl;
	return NULL;
}

BEZIER * TRACK::GetLapSequence(int idx)
{
	if (idx < NumSectors() && idx >= 0)
		return lapsequence[idx];
	else return NULL;
}
