#include "objects.h"

OBJECTS::OBJECTS()
{
	error_log.open((settings.GetSettingsDir() + "/logs/objects.log").c_str());
	
	object_list = NULL;
	model_list = NULL;
}

OBJECTS::~OBJECTS()
{
	error_log.close();
	
	//deallocate objects and models
	DeleteAll();
}

void OBJECTS::DeleteAll()
{
	for (map <string, GLuint>::iterator i = texture_db.begin(); i != texture_db.end(); i++)
	{
		glDeleteTextures(1, &(i->second));
	}
	
	texture_db.clear();
	
	while (object_list != NULL)
		delobject();
	
	while (model_list != NULL)
		delmodel();
}

void OBJECTS::delobject()
{
	if (object_list == NULL)
		return;
	
	//save old tree
	OBJECTNODE * old = object_list;
	object_list = object_list->next;
	delete old;
}

void OBJECTS::delmodel()
{
	if (model_list == NULL)
		return;
	
	//save old tree
	OBJECTMODEL * old = model_list;
	model_list = model_list->next;
	delete old;
}

void OBJECTS::DrawObject(OBJECTNODE * object)
{	
	
	//int i;
	//float yoffset = 0.0f;
	float dx, dy, dz, rc;
	
	//range cull
	VERTEX objcenter = object->model->jmodel.GetBBOX().GetCenter();
	float temp = objcenter.x;
	objcenter.x = objcenter.y;
	objcenter.y = objcenter.z;
	objcenter.z = temp;
	VERTEX objpos = object->pos + objcenter;
	dx=objpos.x+cam.position.x; dy=objpos.y+cam.position.y; dz=objpos.z+cam.position.z;
    rc=dx*dx+dy*dy+dz*dz;
	
	if (!object->model->skybox)
	{
		float lod_far = settings.GetViewDistance()+object->model->jmodel.GetRadius();
		if (rc > lod_far*lod_far) return;
	
		//use different techniques based on range
		//frustum cull!
		float bound, rd;
		//if (rc > lod_far)
		{
			//bound = spread*2.0f;
			bound = object->model->jmodel.GetRadius();
			int i;
			for (i=0;i<6;i++) 
			{
				rd=cam.frustum[i][0]*objpos.x+
				   cam.frustum[i][1]*objpos.y+
				   cam.frustum[i][2]*objpos.z+
				   cam.frustum[i][3];
				if (rd<=-bound)
				{
					return;
				}
			}
		}
	}
	
	glPushMatrix();
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	
	if (object->model->skybox)
	{
		glMatrixMode( GL_PROJECTION );
		glPushMatrix();
		glLoadIdentity( );
		gluPerspective( 45.0f, (float)settings.GetDisplayX()/settings.GetDisplayY(), 0.1f, 10000.0 );
		glMatrixMode( GL_MODELVIEW );
		GLdouble temp_matrix[16];
		cam.PutTransformInto(temp_matrix);
		glLoadMatrixd(temp_matrix);
		glDepthMask(0);
	}
	
	float transform_matrix[16];
	
	object->dir.GetMat(transform_matrix);
	
	glTranslatef(object->pos.x, object->pos.y, object->pos.z);
	glMultMatrixf(transform_matrix);
	glRotated(-90, 1,0,0);
	
	if (object->model != NULL)
	{
		glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1);
		
		glColorMaterial (GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
		glEnable (GL_COLOR_MATERIAL);
		glColor3f (1.0, 1.0, 1.0);
		GLfloat specular [] = { 0.0, 0.0, 0.0, 0.0 };
		glMaterialfv (GL_FRONT_AND_BACK, GL_SPECULAR, specular);
		GLfloat shininess [] = { 0.0 };
		glMaterialfv (GL_FRONT_AND_BACK, GL_SHININESS, shininess);
		
		if (object->model->fullbright)
			glDisable(GL_LIGHTING);
		else
			glEnable(GL_LIGHTING);
		
		glEnable(GL_BLEND);
		//glDisable(GL_DEPTH_TEST);
		glAlphaFunc(GL_GREATER, 0.9f);
		//glAlphaFunc(GL_GEQUAL, 1.0f);
		/*if (object->model->skybox)
		{
			glDepthMask(0);
		}
		else
		{
			glDepthMask(1);
		}*/
		glEnable(GL_ALPHA_TEST);
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
		//glCullFace(GL_FRONT);
		//glEnable(GL_CULL_FACE);
		//glDepthMask(0);
		glColor4f(1,1,1,1);
		
		object->model->jmodel.DrawStatic();
		glDepthMask(1);
		//glEnable(GL_FOG);
	}
	else
		cout << "NULL model" << endl;
	
	if (object->model->skybox)
	{
		glDepthMask(1);
		glMatrixMode( GL_PROJECTION );
		glPopMatrix();
		glMatrixMode( GL_MODELVIEW );
	}
	
	glPopAttrib();
	glPopMatrix();
}

void OBJECTS::Draw()
{
	//setup gl flags
	//glEnable(GL_BLEND);
	//glDisable( GL_TEXTURE_2D );
	/*glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D,treetex[0]);
	glDisable( GL_LIGHTING);
	glAlphaFunc(GL_GREATER, 0.9f);
	glEnable(GL_ALPHA_TEST);
	glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );*/
	//glBlendFunc( GL_ONE, GL_ONE );
		
	OBJECTNODE * curpos = object_list;
	while (curpos != NULL)
	{
		if (curpos->model->skybox)
			DrawObject(curpos);
		curpos = curpos -> next;
	}
	
	curpos = object_list;
	while (curpos != NULL)
	{
		if (!curpos->model->skybox)
			DrawObject(curpos);
		curpos = curpos -> next;
	}

	
	//reset gl flags
	/*glDisable(GL_BLEND);
	glColor4f(1.0f,1.0f,1.0f,1.0f);
	glEnable( GL_TEXTURE_2D );
	glEnable( GL_LIGHTING);
	glDisable(GL_ALPHA_TEST);*/
}

OBJECTNODE * OBJECTS::Add(VERTEX pos, float rotation, string modelname, string texname, bool mip, bool fullbright, bool skybox, bool drv, bool col)
{
	OBJECTNODE * oldfirst = object_list;
	object_list = new OBJECTNODE;
	object_list->next = oldfirst;
	
	object_list->pos.x = pos.x;
	//object_list->pos.y = pos.y;
	object_list->pos.z = pos.z;
	
	object_list->driveable = drv;
	object_list->cancollide = col;
	
	//if (pos.y == -1337)
	{
//		object_list->pos.y = terrain.GetHeight(object_list->pos.x, object_list->pos.z) + pos.y;
	}
	
	//apply rotation (no longer used)
	//object_list->dir.Rotate(rotation, 0, 1, 0);
	
	OBJECTMODEL * mplist = model_list;
	
	bool found = false;
	int count = 0;
	while (mplist != NULL && !found)
	{
		//cout << mplist->name << "," << modelname << endl;
		if (mplist->name == modelname)
		{
			found = true;
			object_list->model = mplist;
		}
		mplist = mplist -> next;
		count++;
	}
	
	if (!found)
	{
		object_list->model = AddModel(modelname, texname, mip, fullbright, skybox);
	}
	
	return object_list;
}

OBJECTNODE::OBJECTNODE()
{
	dir.LoadMultIdent();
}

OBJECTMODEL * OBJECTS::AddModel(string modelname, string texname, bool mip, bool fullbright, bool skybox)
{
	OBJECTMODEL * oldfirst = model_list;
	model_list = new OBJECTMODEL;
	model_list->next = oldfirst;
	
	model_list->name = modelname;
	model_list->jmodel.Load(path + "/" + modelname, false);
	
	model_list->fullbright = fullbright;
	
	model_list->skybox = skybox;
	
	//search for the texture in our texture db
	map <string, GLuint>::iterator tslot = texture_db.find(texname);
	if (tslot == texture_db.end())
	{
		GLuint newtexid = utility.TexLoad(path + "/" + texname, mip);
		texture_db[texname] = newtexid;
		//cout << texname << ": " << newtexid << endl;
		model_list->jmodel.TextureID(newtexid, 0);
	}
	else
	{
		model_list->jmodel.TextureID(tslot->second, 0);
	}
	
	//cout << "Addmodel: " << modelname << endl;
	return model_list;
}

extern void LoadingScreen(string loadtext);

void OBJECTS::LoadObjectsFromFolder(string objectpath)
{
	DeleteAll();
	collision.Clear();
	
	OBJECTNODE * added = NULL;
	
	ifstream o;
	
	o.open((objectpath+"/list.txt").c_str());
	
	path = objectpath;
	
	if (o)
	{
		string m;
		string t;
		bool mip;
		bool fb, sb;
		VERTEX p;
		float r;
		bool c, d;
		
		int count = 0;
		
		while (!o.eof())
		{
			if (count % 100 == 0)
			{
				//char buffer[256];
				//sprintf(buffer, "Loading...\nLoading scenery objects\n%i", count);
				//LoadingScreen(buffer);
				string msg = "Loading...\nLoading scenery objects";
				for (int sn = 0; sn < count/100; sn++)
					msg = msg + ".";
				LoadingScreen(msg);
			}
			
			m = utility.sGetParam(o);
			t = utility.sGetParam(o);
			mip = utility.bGetParam(o);
			fb = utility.bGetParam(o);
			sb = utility.bGetParam(o);
			p.x = utility.fGetParam(o);
			p.y = utility.fGetParam(o);
			p.z = utility.fGetParam(o);
			//r = utility.fGetParam(o);
			r = 0;
			d = utility.bGetParam(o);
			c = utility.bGetParam(o);
			
			added = NULL;
			
			if (m != "" && m != utility.GetEOFString())
			{
				added = Add(p, r, m, t, mip, fb, sb, d, c);
				count++;
				//cout << "added object " << m << "...";
				unsigned int i;
				for (i = 0; i < added->model->jmodel.GetFaces(); i++)
				{
					/*short vi[3];
					VERTEX tri[3];
					VERTEX norms[3];
					for (unsigned int v = 0; v < 3; v++)
					{
						vi[v] = added->model->jmodel.GetFace(i)[v];
						tri[v].Set(added->model->jmodel.GetVert(vi[v]));
						norms[v].Set(added->model->jmodel.GetNorm(added->model->jmodel.GetNormIdx(i)[v]));
					}
					VERTEX norm;
					for (unsigned int v = 0; v < 3; v++)
						norm = norm + norms[v];
					norm = norm.normalize();
					VERTEX tnorm = (tri[2] - tri[0]).cross(tri[1] - tri[0]);
					if (norm.dot(tnorm) > 0)
					{
						short tvi = vi[1];
						vi[1] = vi[2];
						vi[2] = tvi;
					}
					collision.AddColNode(added, vi);*/
					collision.AddColNode(added, added->model->jmodel.GetFace(i));
				}
				//cout << "done" << endl;
			}
		}
		
		o.close();
		
		collision.GenerateCollisionTree();
	}
	else
	{
		error_log << "Couldn't open Object List: " << objectpath << "/list.txt" << endl;
	}
}

bool OBJECTS::Collide(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal, float seglen)
{
	return collision.CollideAABB(origin, direction, outtri, closest, normal, seglen);
}

bool OBJECTS::CollideD(VERTEXD origin, VERTEXD direction, VERTEXD &outtri, bool closest, VERTEXD & normal, double seglen)
{
	return collision.CollideAABB_double(origin, direction, outtri, closest, normal, seglen);
}

bool OBJECTS::CollideDriveable(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal)
{
	return collision.CollideDriveable(origin, direction, outtri, closest, normal);
}

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

OBJCOLNODE::OBJCOLNODE()
{
	object = NULL;
	int i;
	for (i = 0; i < 3; i++)
		vertexIndex[i] = 0;
}

bool OBJCOLNODE::operator==(const OBJCOLNODE & other)
{
	return (object == other.object && vertexIndex[0] == other.vertexIndex[0] &&
			vertexIndex[1] == other.vertexIndex[1] &&
			vertexIndex[2] == other.vertexIndex[2]);
}

OBJCOLNODE::OBJCOLNODE(const OBJCOLNODE & other)
{
	object = other.object;
	vertexIndex[0] = other.vertexIndex[0];
	vertexIndex[1] = other.vertexIndex[1];
	vertexIndex[2] = other.vertexIndex[2];
}

OBJCOLNODE& OBJCOLNODE::operator= (const OBJCOLNODE &other)
{
	object = other.object;
	vertexIndex[0] = other.vertexIndex[0];
	vertexIndex[1] = other.vertexIndex[1];
	vertexIndex[2] = other.vertexIndex[2];
	
	return *this;
}

void OBJECTCOLLISION::AddColNode(OBJECTNODE * newobject, short * newvi)
{
	OBJCOLNODE temp;
	temp.object = newobject;
	int i;
	for (i = 0; i < 3; i++)
		temp.vertexIndex[i] = newvi[i];
	
	if (COLLIDE_AND_DRIVE_TOGETHER)
	{
		if (newobject->cancollide || newobject->driveable)
		{
			colnodes.push_back(temp);
			drvnodes.push_back(temp);
		}
	}
	else
	{
		if (newobject->cancollide)
		{
			colnodes.push_back(temp);
		}
		
		if (newobject->driveable)
		{
			drvnodes.push_back(temp);
		}
	}
}

bool OBJECTCOLLISION::CollideDriveable(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal)
{
	list <OBJCOLNODE>::iterator i1 = drvnodes.begin();
	
	bool hadcollision = false;
	VERTEX curtri[3], tvert;
	int retval;
	float t,u,v;
	
	origin.z = -origin.z;
	
	int count = 0;
	
	while (i1 != drvnodes.end())
	{
		//collide = curnode->model->jmodel.Collide(origin - curnode->pos, direction, tvert, closest);
		count++;
		
		curtri[0].Set(i1->object->model->jmodel.GetVert(i1->vertexIndex[0]));
		curtri[1].Set(i1->object->model->jmodel.GetVert(i1->vertexIndex[1]));
		curtri[2].Set(i1->object->model->jmodel.GetVert(i1->vertexIndex[2]));
		
		//curtri = curtri + ((VERTEX) (i1->object)->pos);
		int n;
		for (n = 0; n < 3; n++)
			curtri[n] = curtri[n] + i1->object->pos;
			
		retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
			curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
			&t, &u, &v);
		
		if (retval)
		{
			if (t < 0)
			{
				//no collision
				retval = 0;
			}
			else
			{
				//collision
				tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
				
				//calculate normal
				curtri[0].z = -curtri[0].z;
				curtri[1].z = -curtri[1].z;
				curtri[2].z = -curtri[2].z;
				normal = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
				normal = normal.normalize();
				
				if (normal.dot(direction) > 0)
					normal.Scale(-1);
				
				if (!closest)
				{
					outtri = tvert;
					
					//implicit caching
					OBJCOLNODE temp;
					temp = *i1;
					
					//cout << colnodes.size() << endl;
					//cout << count << endl;
					
					drvnodes.erase(i1);
					drvnodes.insert(drvnodes.begin(), temp);
					
					return true;
				}
			}
		}

		//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
		//tvert = ;
		if (retval && closest)
		{
			if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
			{
				//outtri = tvert + curnode->pos;
				outtri = tvert;
			}
			
			hadcollision = true;
		}
		
		i1++;
	}
	
	if (closest && hadcollision)
	{
		return true;
	}
	
	//cout << "nocol: " << count << endl;
	
	return false;
}

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

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

void OBJECTCOLLISION::GenerateCollisionTree()
{
	bool verbose = false;
	
	if (verbose)
		cout << "Generating collision tree..." << endl;
	
	coltree.DeleteChildren();
	
	if (verbose)
		cout << "deleted old tree" << endl;
	
	coltree.left = NULL;
	coltree.right = NULL;
	
	coltree.leaves.clear();
	
	//while (curnode != NULL)
	int count = 0;
	
	list <OBJCOLNODE>::iterator i = colnodes.begin();
	for (i = colnodes.begin(); i != colnodes.end(); i++)
	{
		coltree.leaves.push_back(&(*i));
		count++;
	}
	
	if (verbose)
		cout << "copied leaves: " << count << endl;
	
	GenerateBranches(&coltree);
	
	if (verbose)
		cout << "done" << endl;
}

void OBJECTCOLLISION::GenerateBranches(OBJCOLBRANCH * branch)
{
	bool verbose = false;
	
	list <OBJCOLNODE *>::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 < 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[3];
		p[0].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0]));
		p[1].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1]));
		p[2].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2]));
		
		int i, c;
		for (c = 0; c < 3; 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];
		pcenter.Scale(0.333333);
		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 > TRIANGLES_PER_BBOX)
	{
		//create children
		branch->left = new OBJCOLBRANCH;
		branch->right = new OBJCOLBRANCH;
		
		int distributor = 0; //hack to stop infinite recursion
		
		//throw leaves into children
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			VERTEX p[3];
			p[0].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0]));
			p[1].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1]));
			p[2].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2]));
			
			VERTEX pcenter = p[0] + p[1] + p[2];
			pcenter.Scale(0.333333);
			
			if (pcenter.dot(axismask) - avgcenter.dot(axismask) > 0.0)
			{
				branch->right->leaves.push_back(*i1);
				distributor = 1;
			}
			else if (pcenter.dot(axismask) - avgcenter.dot(axismask) < 0.0)
			{
				branch->left->leaves.push_back(*i1);
				distributor = 0;
			}
			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 if 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;
	}
}

void OBJECTCOLLISION::Clear()
{
	coltree.DeleteChildren();
	colnodes.clear();
	drvnodes.clear();
}

OBJECTCOLLISION::~OBJECTCOLLISION()
{
	Clear();
}

bool OBJECTCOLLISION::CollideModelAABB(VERTEX * modelverts, int numfaces, AABB modelbbox, VERTEX & outtri, bool closest, VERTEX & normal, float & depth)
{
	normal.zero();
	
	bool col = CollideBranchModel(modelverts, numfaces, modelbbox, outtri, closest, &coltree, normal, depth);
	
	normal = normal.normalize();
	
	return col;
}

bool OBJECTCOLLISION::CollideAABB(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, VERTEX & normal, float seglen)
{
	bool col = false;
	
	/*if (lastcolpatch != NULL)
	{
		col = lastcolpatch->BEZIER_COLLIDE_FUNCTION(origin, direction, outtri);
		if (col && !closest)
		{
			colpatch = lastcolpatch;

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

bool OBJECTCOLLISION::CollideAABB_double(VERTEXD origin, VERTEXD direction, VERTEXD &outtri, bool closest, VERTEXD & normal, double seglen)
{
	bool col = false;
	
	origin.z = -origin.z;
	direction.z = -direction.z;
	
	col = CollideBranch_double(origin, direction, outtri, closest, &coltree, normal, seglen);
	
	return col;
}

bool OBJECTCOLLISION::CollideBranch(VERTEX origin, VERTEX direction, VERTEX &outtri, bool closest, OBJCOLBRANCH * branch, VERTEX & normal, float seglen)
{
	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 hadcollision = false;
		VERTEX curtri[3], tvert;
		VERTEX tnorm;
		float t,u,v;
		
		int retval = 0;
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			curtri[0].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0]));
			curtri[1].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1]));
			curtri[2].Set((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2]));
			
			//curtri = curtri + ((VERTEX) (i1->object)->pos);
			int n;
			for (n = 0; n < 3; n++)
				curtri[n] = curtri[n] + (*i1)->object->pos;
				
			retval = INTERSECT_FUNCTION(origin.v3(), direction.v3(),
				curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
				&t, &u, &v);
			
			if (retval)
			{
				if (t < 0)
				{
					//no collision
					retval = 0;
				}
				else
				{
					//collision
					tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
					tvert.z = -tvert.z;
					
					//calculate normal
					curtri[0].z = -curtri[0].z;
					curtri[1].z = -curtri[1].z;
					curtri[2].z = -curtri[2].z;
					normal = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
					normal = normal.normalize();
					
					if (normal.dot(direction) > 0)
						normal.Scale(-1);
					
					/*direction.DebugPrint();
					normal.DebugPrint();
					cout << endl;*/
					
					if (!closest)
					{
						outtri = tvert;
						
						return true;
					}
				}
			}
	
			//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
			//tvert = ;
			if (retval && closest)
			{
				if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
				{
					//outtri = tvert + curnode->pos;
					outtri = tvert;
				}
				
				hadcollision = true;
			}
		}
		
		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 (!branch->bbox.IntersectSegment(origin, direction, seglen))
		return false;
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	bool rcol = false;
	bool lcol = false;

	VERTEX r_outtri, l_outtri;
	
	if (right != NULL)
		rcol = CollideBranch(origin, direction, r_outtri, closest, branch->right, normal, seglen);
		
	//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, branch->left, normal, seglen);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		outtri = l_outtri;
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		outtri = r_outtri;
		return rcol;
	}
	
	//collision on both branches
	if (lcol && rcol)
	{
		if ((origin - r_outtri).len() < (origin - l_outtri).len())
		{
			outtri = r_outtri;
			return rcol;
		}
		else
		{
			outtri = l_outtri;
			return lcol;
		}
	}
	
	return false;
}

bool OBJECTCOLLISION::CollideBranch_double(VERTEXD origin, VERTEXD direction, VERTEXD &outtri, bool closest, OBJCOLBRANCH * branch, VERTEXD & normal, double seglen)
{
	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 hadcollision = false;
		VERTEXD curtri[3], tvert;
		VERTEXD tnorm;
		double t,u,v;
		
		int retval = 0;
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			curtri[0].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0])));
			curtri[1].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1])));
			curtri[2].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2])));
			
			//curtri = curtri + ((VERTEX) (i1->object)->pos);
			int n;
			for (n = 0; n < 3; n++)
				curtri[n] = curtri[n] + (*i1)->object->pos;
				
			retval = utility.IntersectTriangleD(origin.v3(), direction.v3(),
				curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
				&t, &u, &v);
			
			if (retval)
			{
				if (t < 0)
				{
					//no collision
					retval = 0;
				}
				else
				{
					//collision...?
					tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
					
					//if ((tvert - origin).len() > seglen || (tvert-origin).dot(direction) <= 0)
					double dist = (tvert-origin).dot(direction);
					if (dist <= 0 || dist > seglen)
					{
						/*cout << "Discarded: " << endl;
						tvert.DebugPrint();
						origin.DebugPrint();
						direction.DebugPrint();
						cout << (tvert-origin).dot(direction) << endl << endl;*/
						
						retval = 0;
					}
					else
					{
						/*cout << "Not discarded: " << endl;
						tvert.DebugPrint();
						origin.DebugPrint();
						direction.DebugPrint();
						cout << (tvert-origin).dot(direction) << endl << endl;*/
						
						//calculate normal
						curtri[0].z = -curtri[0].z;
						curtri[1].z = -curtri[1].z;
						curtri[2].z = -curtri[2].z;
						tnorm = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
						tnorm = tnorm.normalize();
						VERTEXD tdir;
						tdir.Set(direction.x, direction.y, -direction.z);
						if (tnorm.dot(tdir) > 0)
							tnorm.Scale(-1);
						
						/*direction.DebugPrint();
						normal.DebugPrint();
						cout << endl;*/
						
						if (!closest)
						{
							tvert.z = -tvert.z;
							outtri = tvert;
							normal = tnorm;
							return true;
						}
					}
				}
			}
	
			//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
			//tvert = ;
			if (retval && closest)
			{
				if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
				{
					//outtri = tvert + curnode->pos;
					tvert.z = -tvert.z;
					outtri = tvert;
					normal = tnorm;
				}
				
				hadcollision = true;
			}
		}
		
		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;*/
	
	VERTEX originf, directionf;
	float seglenf;
	seglenf = seglen;
	originf.Set(origin.x, origin.y, origin.z);
	directionf.Set(direction.x, direction.y, direction.z);
	
	if (!branch->bbox.IntersectSegment(originf, directionf, seglenf))
		return false;
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	bool rcol = false;
	bool lcol = false;

	VERTEXD r_outtri, l_outtri;
	
	if (right != NULL)
		rcol = CollideBranch_double(origin, direction, r_outtri, closest, branch->right, normal, seglen);
		
	//collision on the right branch
	/*if (rcol && !closest)
	{
		outtri = r_outtri;
		colpatch = r_colpatch;
		return rcol;
	}*/
	
	if (left != NULL)
		lcol = CollideBranch_double(origin, direction, l_outtri, closest, branch->left, normal, seglen);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		outtri = l_outtri;
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		outtri = r_outtri;
		return rcol;
	}
	
	//collision on both branches
	if (lcol && rcol)
	{
		VERTEXD originz;
		originz.Set(origin.x, origin.y, -origin.z);
		
		if ((originz - r_outtri).len() < (originz - l_outtri).len())
		{
			outtri = r_outtri;
			return rcol;
		}
		else
		{
			outtri = l_outtri;
			return lcol;
		}
	}
	
	return false;
}

bool OBJECTCOLLISION::CollideBranchModel(VERTEX * modelverts, int numfaces, AABB & modelbbox, VERTEX & outtri, bool closest, OBJCOLBRANCH * branch, VERTEX & normal, float & depth)
{
	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)
	{
		//cout << "Leaf " << endl;
		VERTEX cartri[3];
		VERTEX objecttri[3];
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			objecttri[0].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0])));
			objecttri[1].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1])));
			objecttri[2].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2])));
			int n;
			for (n = 0; n < 3; n++)
				objecttri[n] = objecttri[n] + (*i1)->object->pos;
		
			int f, v;
			
			for (f = 0; f < numfaces; f++)
			{
				for (v = 0; v < 3; v++)
					cartri[v] = modelverts[f*3+v];
				
				int which;
				VERTEX colpt, coldest, colorig;
				int retval;
				retval = utility.BruteForceTriangleIntersectionF(cartri, objecttri, colpt, coldest, colorig, which);
				
				//if (f == 0)
				if (0)
				{
					for (n = 0; n < 3; n++)
						objecttri[n].DebugPrint();
					
					cout << "---" << endl;
					
					for (n = 0; n < 3; n++)
						cartri[n].DebugPrint();
					
					cout << endl;
				}
				
				if (retval)
				{
					VERTEX tcolpt;
					tcolpt.Set(colpt.x, colpt.y, -colpt.z);
					VERTEX tcoldest;
					tcoldest.Set(coldest.x, coldest.y, -coldest.z);
					VERTEX tcolorig;
					tcolorig.Set(colorig.x, colorig.y, -colorig.z);
					
					VERTEX tobjecttri[3], tcartri[3];
					VERTEX tnormd, tnorm;
					//cout << "object tri: " << endl;
					for (n = 0; n < 3; n++)
					{
						tcartri[n].Set(cartri[n].x, cartri[n].y, -cartri[n].z);
						tobjecttri[n].Set(objecttri[n].x, objecttri[n].y, -objecttri[n].z);
						//tobjecttri[n].DebugPrint();
					}
					
					/*VERTEX bigsurf[3];
					for (n = 0; n < 3; n++)
					{
						if (which == 0)
							bigsurf[n].Set(cartri[n].x, cartri[n].y, -cartri[n].z);
						else
							bigsurf[n].Set(objecttri[n].x, objecttri[n].y, -objecttri[n].z);
					}
					tnorm = (bigsurf[2] - bigsurf[0]).cross(bigsurf[1] - bigsurf[0]);*/
					//tnorm = (objecttri[2] - objecttri[0]).cross(objecttri[1] - objecttri[0]);
					
					/*if (which == 1)
					{
						tnorm = (objecttri[2] - objecttri[0]).cross(objecttri[1] - objecttri[0]);
						tnorm = tnorm.normalize();
					}*/
					
					tnorm = (tobjecttri[2] - tobjecttri[0]).cross(tobjecttri[1] - tobjecttri[0]);
					tnorm = tnorm.normalize();
					
					if (which == 0)
					{
						tnormd = (tcartri[2] - tcartri[0]).cross(tcartri[1] - tcartri[0]);
						tnormd = tnormd.normalize();
					}
					else
						tnormd = tnorm;
					
					tnormd = tnorm;
					
					//hack to try to fix normals that are pointing the wrong way
					VERTEX carcenter = modelbbox.GetCenter();
					carcenter.z = -carcenter.z;
					VERTEX tdir;
					tdir = tcolpt - carcenter;
					if (tnorm.dot(tdir) > 0)
						tnorm.Scale(-1);
					if (tnormd.dot(tdir) > 0)
						tnormd.Scale(-1);
					
					//adjust the collision info to make sure the depth is
					// into the surface
					if ((tcoldest-tcolorig).dot(tnormd) > 0)
					{
						VERTEX tvert;
						tvert = tcoldest;
						tcoldest = tcolorig;
						tcolorig = tvert;						
					}
					
					//find the depth
					float tdepth = (tcolpt - tcoldest).dot(tnormd);
					
					//cout << tdepth << endl;
					
					/*VERTEX ncolseg;
					ncolseg = tcolseg;
					if (tcolseg.dot(tnorm) < 0)
						ncolseg.Scale(-1);
					float tdepth = ncolseg.dot(tnorm);*/
					
					//if (!closest)
					{
						outtri = tcolpt;
						normal = tnorm;
						//normal = tnormd;
						depth = tdepth;

						/*tcolorig.DebugPrint();
						tcoldest.DebugPrint();
						outtri.DebugPrint();
						tnormd.DebugPrint();
						normal.DebugPrint();
						//carcenter.DebugPrint();
						cout << depth << endl << endl;*/
						
						return true;
					}
				}
			}
		}
		
		return false;
		
		/*bool hadcollision = false;
		VERTEXD curtri[3], tvert;
		VERTEXD tnorm;
		double t,u,v;
		
		int retval = 0;
		
		list <OBJCOLNODE *>::iterator i1 = branch->leaves.begin();
		
		for (i1 = branch->leaves.begin(); i1 != branch->leaves.end(); i1++)
		{
			curtri[0].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[0])));
			curtri[1].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[1])));
			curtri[2].Set(((*i1)->object->model->jmodel.GetVert((*i1)->vertexIndex[2])));
			
			//curtri = curtri + ((VERTEX) (i1->object)->pos);
			int n;
			for (n = 0; n < 3; n++)
				curtri[n] = curtri[n] + (*i1)->object->pos;
				
			retval = utility.IntersectTriangleD(origin.v3(), direction.v3(),
				curtri[0].v3(), curtri[1].v3(), curtri[2].v3(), 
				&t, &u, &v);
			
			if (retval)
			{
				if (t < 0)
				{
					//no collision
					retval = 0;
				}
				else
				{
					//collision...?
					tvert = curtri[0].ScaleR(1-u-v) + curtri[1].ScaleR(u) + curtri[2].ScaleR(v);
					
					//if ((tvert - origin).len() > seglen || (tvert-origin).dot(direction) <= 0)
					double dist = (tvert-origin).dot(direction);
					if (dist <= 0 || dist > seglen)
					{
						
						retval = 0;
					}
					else
					{
						
						//calculate normal
						curtri[0].z = -curtri[0].z;
						curtri[1].z = -curtri[1].z;
						curtri[2].z = -curtri[2].z;
						tnorm = (curtri[2] - curtri[0]).cross(curtri[1] - curtri[0]);
						tnorm = tnorm.normalize();
						VERTEXD tdir;
						tdir.Set(direction.x, direction.y, -direction.z);
						if (tnorm.dot(tdir) > 0)
							tnorm.Scale(-1);
						
						if (!closest)
						{
							tvert.z = -tvert.z;
							outtri = tvert;
							normal = tnorm;
							return true;
						}
					}
				}
			}
	
			//collide = curnode->model->jmodel.Collide(curnode->pos - origin, direction, tvert, closest);
			//tvert = ;
			if (retval && closest)
			{
				if ((hadcollision && (origin - tvert).len() < (origin - outtri).len()) || !hadcollision)
				{
					//outtri = tvert + curnode->pos;
					tvert.z = -tvert.z;
					outtri = tvert;
					normal = tnorm;
				}
				
				hadcollision = true;
			}
		}
		
		if (closest && hadcollision)
		{
			return true;
		}
		
		return false;*/
	}
	
	if (verbose)
	{
		branch->bbox.DebugPrint();
		modelbbox.DebugPrint();
		cout << endl;
	}
	
	//check bounding box
	/*if (!branch->bbox.IntersectRay(origin, direction))
		return false;*/
		
	if (!branch->bbox.IntersectAABB(modelbbox))
		return false;
	
	if (verbose)
		cout << "BBOX collision" << endl;
	
	//modelbbox.DebugPrint();
	
	bool rcol = false;
	bool lcol = false;

	VERTEX r_outtri, l_outtri;
	VERTEX r_normal, l_normal;
	float r_depth, l_depth;
	
	if (right != NULL)
		rcol = CollideBranchModel(modelverts, numfaces, modelbbox, r_outtri, closest, branch->right, r_normal, r_depth);
		
	//collision on the right branch
	/*if (rcol && !closest)
	{
		outtri = r_outtri;
		colpatch = r_colpatch;
		return rcol;
	}*/
	
	if (left != NULL)
		lcol = CollideBranchModel(modelverts, numfaces, modelbbox, l_outtri, closest, branch->left, l_normal, l_depth);
	
	//collision on the left branch only
	if (lcol && !rcol)
	{
		outtri = l_outtri;
		normal = normal + l_normal;
		depth = l_depth;
		return lcol;
	}
	
	//collision on the right branch only
	if (rcol && !lcol)
	{
		outtri = r_outtri;
		normal = normal + r_normal;
		depth = r_depth;
		return rcol;
	}
	
	//collision on both branches
	if (lcol && rcol)
	{	
		//if ((modelbbox.GetCenter() - r_outtri).len() < (modelbbox.GetCenter() - l_outtri).len())
		//if (r_depth > l_depth)
		if (r_normal.y*r_normal.y < l_normal.y*l_normal.y)
		//if (1)
		{
			outtri = r_outtri;
			normal = r_normal;
			//normal = normal + r_normal + l_normal;
			depth = r_depth;
			return rcol;
		}
		else
		{
			outtri = l_outtri;
			normal = l_normal;
			//normal = normal + r_normal + l_normal;
			depth = l_depth;
			return lcol;
		}
		
		/*outtri = (r_outtri + l_outtri).ScaleR(0.5);
		normal = (r_normal + l_normal).normalize();
		depth = (l_depth + r_depth)*0.5;*/
	}
	
	return false;
}
