/***************************************************************************
 *            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;
	}*/
	patchnodes.clear();
}

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 * lastnode = NULL;
	if (!patchnodes.empty())
	{
		lastnode = &(patchnodes.back());
	}
	patchnodes.push_back(newpatch);
	if (lastnode != NULL)
	{
		//cout << lastnode << " attaches to " << &(patchnodes.back()) << endl;
		lastnode->Attach(patchnodes.back());
	}
	return &(patchnodes.back());
}

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

bool ROADSTRIP::ReadFrom(ifstream &openfile)
{
	//optional....
	ClearPatches();
	
	lastcolpatch = NULL;
	
	if (!openfile)
		return false;
	
	BEZIER * newpatch = NULL;
	
	int num;
	
	//the number of patches for this road
	openfile >> num;
	
	int i;
	
	for (i = 0; i < num; i++)
	{
		BEZIER temppatch;
		//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)
	{
		//cout << "close track?  " << num << " - " << newpatch << " - " << newpatch->next_patch << endl;
		newpatch->next_patch = NULL;
		//close a looped track
		if (newpatch->points[0][0].equals(GetFirstPatch()->points[3][0]) && newpatch->points[0][3].equals(GetFirstPatch()->points[3][3]))
		{
			//cout << "closed track " << num << " - " << newpatch << " - " << newpatch->next_patch << endl;
			newpatch->Attach(*GetFirstPatch());
		}
	}
	
	GenerateCollisionTree();
	
	return true;
}

bool ROADSTRIP::WriteTo(ofstream &openfile)
{
	openfile << NumPatches() << endl << endl;
		
	/*BEZIERNODE * curnode = patchnodes;
	
	while (curnode != NULL)
	{
		curnode->patch.WriteTo(openfile);
		openfile << endl;
		
		curnode = curnode->next;
	}*/
	
	for (list <BEZIER>::iterator i = patchnodes.begin(); i != patchnodes.end(); i++)
	{
		i->WriteTo(openfile);
	}
	
	return true;
}

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

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;*/
	
	if (patchnodes.empty())
		return false;
	
	patchnodes.pop_back();
	return true;
}

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;*/
	
	if (patchnodes.empty())
		return NULL;
	return &(patchnodes.back());
}

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;
	
	for (list <BEZIER>::iterator curnode = patchnodes.begin(); curnode != patchnodes.end(); curnode++)
	{
		//i->WriteTo(openfile);
	//}
	//while (curnode != NULL)
	//{
		//if (tot - count < 50)
		bool drawme = true;
		//VERTEX pos = curnode->patch.center+cam.position;
		VERTEX & pos = curnode->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->radius)
			{
				drawme = false;
			}
		}
		
		if (drawme)
		{
			curnode->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)
	for (list <BEZIER>::iterator curnode = patchnodes.begin(); curnode != patchnodes.end(); curnode++)
	{
		//cout << "Patch " << count << endl;
		
		//curnode->patch.Visualize(wireframe, fill, color);
		collide = curnode->BEZIER_COLLIDE_FUNCTION(origin, direction, tvert, tnorm);
		if (collide && !closest)
		{
			outtri = tvert;
			colpatch = &(*curnode);
			
			//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);
			}
			
			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 <BEZIER *>::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)->BEZIER_COLLIDE_FUNCTION(origin, direction, tvert, tnorm);
			if (collide && !closest)
			{
				normal = tnorm;
				outtri = tvert;
				colpatch = *i1;
				
				//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;
				}
				
				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();
	
	for (list <BEZIER>::iterator curnode = patchnodes.begin(); curnode != patchnodes.end(); curnode++)
	//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 <BEZIER *>::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)->points[0][0];
		p[1] = (*i1)->points[0][3];
		p[2] = (*i1)->points[3][3];
		p[3] = (*i1)->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;
	//fudge to improve collision detection for especially bendy strips
	for (int i = 0; i < 3; i++)
	{
		minv[i] -= 0.1;
		maxv[i] += 0.1;
	}
	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)->points[0][0];
			p[1] = (*i1)->points[0][3];
			p[2] = (*i1)->points[3][3];
			p[3] = (*i1)->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;
	verticaltrackingskyboxes = true;
	direction = true;
}

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(0); //to do - create a proper loop
	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, bool reverse)
{
	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;
	int index = 0;
	bool end_of_position = false;
	startloc.clear();
	trackconfig.SuppressError(true); //suppress error message 
	while (!end_of_position)
	{
		std::stringstream s;
		s<<"start position "<<index;
		if (trackconfig.GetParam(s.str(), tvert))
		{
			sl.Set(tvert);
			SetStart(sl);
			index++;
		}
		else end_of_position = true;
	}
	trackconfig.SuppressError(false); //turn error message back on

	if (index == 0) //still using the old format
	{
		trackconfig.GetParam("start position", tvert);	
		sl.Set(tvert);
		SetStart(sl);
	}

	string strcullfaces;
	trackconfig.GetParam("cull faces", strcullfaces);
	//sl.Set(tvert);
	
	if (strcullfaces == "no")
		cullfaces = false;
	else
		cullfaces = true;
	
	string strhoriz;
	trackconfig.GetParam("vertical tracking skyboxes", strhoriz);
	if (strhoriz == "no")
		verticaltrackingskyboxes = false;
	else
		verticaltrackingskyboxes = true;
	objects.SetVerticalSkyboxTracking(verticaltrackingskyboxes);
	
	int numroads, i;
	
	/*trackfile >> sl.x;
	trackfile >> sl.y;
	trackfile >> sl.z;*/
	
	//SetStart(sl);
	
	index = 0;
	end_of_position = false;
	startquat.clear();
	VERTEX sov;
	float w;
	trackconfig.SuppressError(true); //suppress error message 
	while (!end_of_position)
	{
		std::stringstream s1,s2;
		s1<<"start orientation-xyz "<<index;
		s2<<"start orientation-w "<<index;
		if (trackconfig.GetParam(s1.str(), tvert) &&
		    trackconfig.GetParam(s2.str(), w))
		{
			sov.Set(tvert);
			QUATERNION so;
			so.x = sov.x;
			so.y = sov.y;
			so.z = sov.z;
			so.w = w;
			SetStartOrientation(so);
			index++;
		}
		else end_of_position = true;
	}
	trackconfig.SuppressError(false); //turn error message back on

	if (index == 0) //still using the old format
	{
		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();
	for (i = 0; i < numroads && trackfile; i++)
	{
		ROADSTRIP * 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]));
		}
	}
	
	if (reverse)
	{
		Reverse();
		direction = false;
	}
	else direction = true;
	
	float f;
	trackconfig.GetParam("non-treaded friction coefficient", f);
	friction1 = f;
	trackconfig.GetParam("treaded friction coefficient", f);
	friction2 = f;

	// calculate distance from starting line for each patch to account for those tracks
	// where starting line is not on the 1st patch of the road
	// note this only update the road with lap sequence 0 on it
	BEZIER* start_patch = GetLapSequence(0);
	if (start_patch)
	{
		start_patch->dist_from_start = 0.0;
		BEZIER* curr_patch = start_patch->next_patch;
		float total_dist = start_patch->length;
		int count = 0;
		while ( curr_patch && curr_patch != start_patch)
		{
			count++;
			curr_patch->dist_from_start = total_dist;
			total_dist += curr_patch->length;
			curr_patch = curr_patch->next_patch;
		}
	}
}

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;
		throw string( "Detected NaN in origin vector" );
	}
	
	if (!(seglen < 0 || seglen >= 0))
	{
		cout << "Detected NaN in seglen" << endl;
		throw string( "Detected NaN in seglen" );
	}
	
	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;
			std::ostringstream error;
			error << "Detected NaN in normal vector " << bezcol << " " << modelcol;
			throw string( error.str() );
			normal.Set(0,1,0);
		}
		
		if (!(elev < 0 || elev >= 0))
		{
			std::ostringstream error;
			error << "Detected NaN in elevation " << bezcol << " " << modelcol;
			throw string( error.str() );
			//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)
	{
		//BEZIER * curpatch = curnode->road.GetFirstNode();
		for (list <BEZIER>::iterator curpatch = curnode->road.patchnodes.begin(); curpatch != curnode->road.patchnodes.end(); curpatch++)
		//while (curpatch != NULL)
		{
			if (curpatch->points[0][0].equals(corners[0]) && 
				curpatch->points[0][3].equals(corners[1]) && 
				curpatch->points[3][0].equals(corners[2]) && 
				curpatch->points[3][3].equals(corners[3]))
			{
				return &(*curpatch);
			}
			
			//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)
			for (list <BEZIER>::iterator curpatch = curnode->road.patchnodes.begin(); curpatch != curnode->road.patchnodes.end(); curpatch++)
			{
				if (pidx == pi)
				{
					return &(*curpatch);
				}
				
				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;
}

void TRACK::Reverse()
{
	//move timing sector 0 back 1 patch
	if (lapsequence.size() > 0)
	{
		BEZIER * startingline = lapsequence[0];
		BEZIER * newstartingline = NULL;
		ROADSTRIPNODE * curnode = roads;
		
		while (curnode != NULL)
		{
			for (list <BEZIER>::iterator curpatch = curnode->road.patchnodes.begin(); curpatch != curnode->road.patchnodes.end(); curpatch++)
			{
				if (curpatch->next_patch == startingline)
				{
					newstartingline = &(*curpatch);
				}
			}
			curnode = curnode->next;
		}
		
		if (newstartingline != NULL)
		{
			//cout << "SUCCESS" << endl;
			lapsequence[0] = newstartingline;
		}
	}
	
	//reverse start positions
	for (vector <QUATERNION>::iterator i = startquat.begin(); i != startquat.end(); i++)
	{
		//*i = i->ReturnConjugate();
		i->Rotate(3.141593, 0,1,0);
	}
	
	//reverse roads
	ROADSTRIPNODE * curnode = roads;
	//list <ROADSTRIPNODE *> roadlist;
	while (curnode != NULL)
	{
		//roadlist.push_back(curnode);
		curnode->road.Reverse();
		curnode = curnode->next;
	}
	/*roadlist.reverse();
	cout << roadlist.size() << endl;
	curnode = roads;
	for (list <ROADSTRIPNODE *>::iterator i = roadlist.begin(); i != roadlist.end(); i++)
	{
		curnode = *i;
		list <ROADSTRIPNODE *>::iterator nexti = i;
		nexti++;
		if (nexti == roadlist.end())
			curnode->next = NULL;
		else
			curnode->next = *nexti;
		
		if (curnode->next != NULL)
			curnode = curnode->next;
	}*/
	
	//reverse the timing sectors
	if (lapsequence.size() > 0)
	{
		list <BEZIER *> revlapsequence;
		for (unsigned int i = 0; i < lapsequence.size(); i++)
		{
			revlapsequence.push_back(lapsequence[i]);
		}
		revlapsequence.reverse();
		BEZIER * startpatch = revlapsequence.back();
		revlapsequence.pop_back();
		revlapsequence.push_front(startpatch);
		list <BEZIER *>::iterator li = revlapsequence.begin();
		for (unsigned int i = 0; i < lapsequence.size() && li != revlapsequence.end(); i++,li++)
		{
			lapsequence[i] = *li;
		}
	}
	
	ReverseStartingPositions();
}

void ROADSTRIP::Reverse()
{
	patchnodes.reverse();
	
	for (list <BEZIER>::iterator i = patchnodes.begin(); i != patchnodes.end(); i++)
	{
		//i->Reverse();
		i->dist_from_start = 0.0;
	}
	
	//fix pointers to next patches for AI
	for (list <BEZIER>::iterator i = patchnodes.begin(); i != patchnodes.end(); i++)
	{
		list <BEZIER>::iterator n = i;
		n++;
		BEZIER * nextpatchptr = NULL;
		if (n != patchnodes.end())
		{
			nextpatchptr = &(*n);
			i->Attach(*nextpatchptr, true);
		}
		else
			i->next_patch = nextpatchptr;
	}
}

void TRACK::ReverseStartingPositions()
{
	list <VERTEX> revloc;
	list <QUATERNION> revquat;
	
	for (vector <VERTEX>::iterator i = startloc.begin(); i != startloc.end(); i++)
		revloc.push_back(*i);
	revloc.reverse();
	startloc.clear();
	for (list <VERTEX>::iterator i = revloc.begin(); i != revloc.end(); i++)
		startloc.push_back(*i);
	
	for (vector <QUATERNION>::iterator i = startquat.begin(); i != startquat.end(); i++)
		revquat.push_back(*i);
	revquat.reverse();
	startquat.clear();
	for (list <QUATERNION>::iterator i = revquat.begin(); i != revquat.end(); i++)
		startquat.push_back(*i);
}
