#include "tinyxml.h"
#include "collisionMgr.h"


using namespace LFA;
using namespace std;
using namespace Ogre;



collisionManager::collisionManager()
{
	mCollision = new std::vector<collisionBranch>;
}

collisionManager::~collisionManager()
{
	if(mCollision)
	{
		delete mCollision;
		mCollision  = 0;
	}
}


void collisionManager::loadCollisionFile( const Ogre::String &collisionFileName, const Ogre::String &groupName )
{
	//clear the mCollision vector
	mCollision->clear();

	TiXmlDocument   *XMLDoc = 0;
	TiXmlElement   *XMLRoot;

	try
	{
		DataStreamPtr pStream = ResourceGroupManager::getSingleton().
			openResource( collisionFileName, groupName );

		String data = pStream->getAsString();
		// Open the .col File
		XMLDoc = new TiXmlDocument();
		XMLDoc->Parse( data.c_str() );
		pStream->close();
		pStream.setNull();

		if( XMLDoc->Error() )
		{
			//We'll just log, and continue on gracefully
			LogManager::getSingleton().logMessage("[collisionLoader] The TiXmlDocument reported an error");
			delete XMLDoc;
			XMLDoc = 0;
			return;
		}
	}
	catch(...)
	{
		//We'll just log, and continue on gracefully
		LogManager::getSingleton().logMessage("[collisionLoader] Error creating TiXmlDocument");
		delete XMLDoc;
		XMLDoc = 0;
		return;
	}

	// Validate the File
	XMLRoot = XMLDoc->RootElement();
	if( String( XMLRoot->Value()) != "col"  ) {
		LogManager::getSingleton().logMessage( "[collisionLoader] Error: Invalid .col File. Missing <col>" );
		delete XMLDoc;
		XMLDoc  = 0;  
		XMLRoot = 0;
		return;
	}

	// parse here into mCollision struct
	TiXmlElement *pElement = XMLRoot->FirstChildElement("branch");
	if(pElement)
	{
		while(pElement)
		{	
			collisionBranch cBranch;
					
			cBranch.name = getAttrib(pElement, "name");
			cBranch.xmin = getAttribReal( pElement, "xmin") ;
			cBranch.ymin = getAttribReal( pElement, "ymin") ;
			cBranch.zmin = getAttribReal( pElement, "zmin") ;
			cBranch.xmax = getAttribReal( pElement, "xmax") ;
			cBranch.ymax = getAttribReal( pElement, "ymax") ;
			cBranch.zmax = getAttribReal( pElement, "zmax") ;

			TiXmlElement *pChildElement = pElement->FirstChildElement("box");
			while(pChildElement)
			{
				collisionBox cBox;

				cBox.name = getAttrib(pChildElement, "name");
				cBox.var = getAttrib(pChildElement, "var");
				cBox.xmin = getAttribReal( pChildElement, "xmin") ;
				cBox.ymin = getAttribReal( pChildElement, "ymin") ;
				cBox.zmin = getAttribReal( pChildElement, "zmin") ;
				cBox.xmax = getAttribReal( pChildElement, "xmax") ;
				cBox.ymax = getAttribReal( pChildElement, "ymax") ;
				cBox.zmax = getAttribReal( pChildElement, "zmax") ;

				cBranch.colBoxes.push_back(cBox);
				pChildElement = pChildElement->NextSiblingElement("box");
			}		

			mCollision->push_back(cBranch);
			pElement = pElement->NextSiblingElement("branch");
		}
	}
	
	LogManager::getSingleton().logMessage( "[collisionLoader] Loaded collision successfully" );
	// Close the XML File
	delete XMLDoc;
	XMLDoc  = 0; 
}


std::vector<collisionHit> collisionManager::pointCollide(Ogre::Vector3 position)
{
	return pointCollide(position.x, position.y, position.z);
}

std::vector<collisionHit> collisionManager::pointCollide(float &x, float &y, float &z)
{
	std::vector<collisionHit> vCollisions;

	if (mCollision)
	{
 		vector<collisionBranch>::iterator branch_iter = mCollision->begin();
		vector<collisionBranch>::iterator branch_end = mCollision->end();
		 
		for(; branch_iter != branch_end; ++branch_iter)
		{
	   		//check for collision here
			if (pointVsBox(x, y, z, (*branch_iter)))
			{
				vector<collisionBox>::iterator box_iter = (*branch_iter).colBoxes.begin();
				vector<collisionBox>::iterator box_end = (*branch_iter).colBoxes.end();

				for(; box_iter != box_end; ++box_iter)
				{
					if (pointVsBox(x, y, z, (*box_iter)))
					{
						collisionHit hit;			// add this box as a hit if there is a collision
						
						hit.name = (*box_iter).name;
						hit.var = (*box_iter).var;
						hit.posX = x;
						hit.posY = y;
						hit.posZ = z;
						hit.qVector.w = 1;
						hit.qVector.x = 0;
						hit.qVector.y = 0;
						hit.qVector.z = 0;
						hit.distance = 0;

						vCollisions.push_back(hit);
					}
				}
			}
		}
	}
	return vCollisions;
}

std::vector<collisionBox> collisionManager::boxCollide(collisionBox &cBox)
{
	return boxCollide(cBox.xmax, cBox.ymax, cBox.zmax, cBox.xmin, cBox.ymin, cBox.zmin);
}

std::vector<collisionBox> collisionManager::boxCollide(float &xmax, float &ymax, float &zmax, float &xmin, float &ymin, float &zmin)
{
	std::vector <collisionBox> vCollisions;

	if (mCollision)
	{
 		vector<collisionBranch>::iterator branch_iter = mCollision->begin();
		vector<collisionBranch>::iterator branch_end = mCollision->end();
		 
		for(; branch_iter != branch_end; ++branch_iter)
		{
	   			//collide box with branches
			if ( boxVsBox((*branch_iter),xmax, ymax,zmax,xmin,ymin,zmin ))
			{
				vector<collisionBox>::iterator box_iter = (*branch_iter).colBoxes.begin();
				vector<collisionBox>::iterator box_end = (*branch_iter).colBoxes.end();

				for(; box_iter != box_end; ++box_iter)
				{
					//TODO allow collision branches to be nested

					//collide box with boxes
					if ( boxVsBox((*box_iter),xmax, ymax,zmax,xmin,ymin,zmin ))
					{
						vCollisions.push_back(*box_iter);
					}
				}
			}
		}
	}
	return vCollisions;
}


std::vector<collisionHit> collisionManager::lineCollide(Ogre::Vector3 position1, Ogre::Vector3 position2)
{
	return lineCollide(position1.x, position1.y, position1.z, position2.x, position2.y, position2.z);
}

std::vector<collisionHit> collisionManager::lineCollide(float &x1, float &y1, float &z1, float &x2, float &y2, float &z2)
{
	std::vector <collisionHit> vCollisions;
	std::vector <collisionHit> hits; //temp variable

	if (mCollision)
	{
		collisionBox rayBBox =  rayToBox(x1,y1,z1,x2,y2,z2);

 		vector<collisionBranch>::iterator branch_iter = mCollision->begin();
		vector<collisionBranch>::iterator branch_end = mCollision->end();
		 
		for(; branch_iter != branch_end; ++branch_iter)
		{
	   		//collide ray bounding box with branches
			if ( boxVsBox((*branch_iter), rayBBox))
			{
				vector<collisionBox>::iterator box_iter = (*branch_iter).colBoxes.begin();
				vector<collisionBox>::iterator box_end = (*branch_iter).colBoxes.end();

				for(; box_iter != box_end; ++box_iter)
				{
					//collide with each box					
					if (boxVsBox((*box_iter), rayBBox))
					{
						hits.clear();
						hits = rayVsBox(x1,y1,z1,x2,y2,z2,(*box_iter));

						if (hits.size())
						{
							vector<collisionHit>::iterator hits_iter = hits.begin();
							vector<collisionHit>::iterator hits_end = hits.end();

							for(; hits_iter != hits_end; ++hits_iter)
							{
								vCollisions.push_back(*hits_iter);
							}
						}
					}
				}
			}
		}
	}

	orderCollisionHitsByDistance (&vCollisions);
	return vCollisions;
}


bool collisionManager::pointVsBox (Ogre::Vector3 position, collisionBox &cBox)
{
	return pointVsBox (position.x, position.y, position.z, cBox.xmax, cBox.ymax, cBox.zmax, cBox.xmin, cBox.ymin, cBox.zmin);
}

bool collisionManager::pointVsBox (float &x, float &y, float &z, collisionBox &cBox)
{
	return pointVsBox (x, y, z, cBox.xmax, cBox.ymax, cBox.zmax, cBox.xmin, cBox.ymin, cBox.zmin);
}
		
bool collisionManager::pointVsBox (float &x, float &y, float &z, float &xmax, float &ymax, float &zmax, float &xmin, float &ymin, float &zmin)
{
	if ((x <= xmax) &&
		(x >= xmin) &&
		(y <= ymax) &&
		(y >= ymin) &&
		(z <= zmax) &&
		(z >= zmin))
	{
		return 1;
	}
	return 0;
}



bool collisionManager::boxVsBox(collisionBox &cBox1, collisionBox &cBox2)
{
	return boxVsBox(cBox1.xmax, cBox1.ymax, cBox1.zmax, cBox1.xmin, cBox1.ymin, cBox1.zmin, cBox2.xmax, cBox2.ymax, cBox2.zmax, cBox2.xmin, cBox2.ymin, cBox2.zmin);
}


bool collisionManager::boxVsBox (collisionBox &cBox, float &xmax, float &ymax, float &zmax, float &xmin, float &ymin, float &zmin)
{
	return boxVsBox(cBox.xmax, cBox.ymax, cBox.zmax, cBox.xmin, cBox.ymin, cBox.zmin,  xmax, ymax, zmax, xmin, ymin, zmin);
}


bool collisionManager::boxVsBox(float &xmax1, float &ymax1, float &zmax1, float &xmin1, float &ymin1, float &zmin1, float &xmax2, float &ymax2, float &zmax2, float &xmin2, float &ymin2, float &zmin2)
{
	//if 2 boxes pass the oneDimentionalTest on all 3 axis there is a collision.
	if ((oneDimentionalTest (xmin1, xmax1, xmin2, xmax2)) &&
		(oneDimentionalTest (ymin1, ymax1, ymin2, ymax2)) &&
		(oneDimentionalTest (zmin1, zmax1, zmin2, zmax2)))
	{
		return 1;
	}

	return 0;
}

std::vector<collisionHit> collisionManager::rayVsBox (Ogre::Vector3 position1, Ogre::Vector3 position2, collisionBox &cBox)
{
	return rayVsBox(position1.x, position1.y, position1.z, position2.x, position2.y, position2.z, cBox);
}

std::vector<collisionHit> collisionManager::rayVsBox (float &x1, float &y1, float &z1, float &x2, float &y2, float &z2, collisionBox &cBox)
{	
	std::vector<collisionHit> hits = rayVsBox(x1, y1, z1, x2, y2, z2, cBox.xmax, cBox.ymax, cBox.zmax, cBox.xmin, cBox.ymin, cBox.zmin);
	
	// add name and var
	if (hits.size())
	{
			vector<collisionHit>::iterator hits_iter = hits.begin();
			vector<collisionHit>::iterator hits_end = hits.end();

			for(; hits_iter != hits_end; ++hits_iter)
			{
				(*hits_iter).name = cBox.name;
				(*hits_iter).var = cBox.var;
			}
	}

	return hits;
}

std::vector<collisionHit> collisionManager::rayVsBox (float &x1, float &y1, float &z1, float &x2, float &y2, float &z2, float &xmax, float &ymax, float &zmax, float &xmin, float &ymin, float &zmin)
{
	std::vector<collisionHit> vCollisions;


	//get vector of given points [Qx-Px, Qy-Py, Qz-Pz]
	Ogre::Vector3 vect;
	vect.x = x2 - x1;
	vect.y = y2 - y1;
	vect.z = z2 - z1;

	collisionHit hit;

	//get t for each plane so we can determine intersection point  (the three will be equal if they are on the same line)
	 //           x – x0   =     y – y0    =    z – z0			 x = x0 + ta
	//   t =     ---------          ---------          ---------			 y = y0 + tb    where    vector = (a, b, c)
    //              a                  b                  c				 z = z0 + tc
	//then test collision with each face of the box.
	float t;

	if (vect.x != 0)  //protect from divide by 0
	{
		t = (xmax - x1) / vect.x ;
				hit.posX = x1 + t * vect.x;
				hit.posY = y1 + t * vect.y;
				hit.posZ = z1 + t * vect.z;
				if (pointVsBox(hit.posX, hit.posY, hit.posZ, xmax, ymax, zmax, xmin, ymin, zmin ))
				{	
					// 0 0 -90
					hit.qVector.w = 0.7071068614;
					hit.qVector.x = 0;
					hit.qVector.y = 0;
					hit.qVector.z = -0.707106701;
					hit.distance = t;
					//hit.distance = sqrt(   pow((hit.posX - x1),2) +  pow((hit.posY - y1),2)  + pow((hit.posZ - z1),2) );  //distance in units
					vCollisions.push_back(hit);
				}


		t = (xmin - x1) / vect.x ;
				hit.posX = x1 + t * vect.x;
				hit.posY = y1 + t * vect.y;
				hit.posZ = z1 + t * vect.z;
				if (pointVsBox(hit.posX, hit.posY, hit.posZ, xmax, ymax, zmax, xmin, ymin, zmin ))
				{
					//0 0 90
					hit.qVector.w = 0.7071068614;
					hit.qVector.x = 0;
					hit.qVector.y = 0;
					hit.qVector.z = 0.707106701;
					hit.distance = t;
					//hit.distance = sqrt(   pow((hit.posX - x1),2) +  pow((hit.posY - y1),2)  + pow((hit.posZ - z1),2) );  //distance in units
					vCollisions.push_back(hit);
				}
	}

	if (vect.y != 0)  //protect from divide by 0
	{
		t = (ymax - y1) / vect.y;
				hit.posX = x1 + t * vect.x;
				hit.posY = y1 + t * vect.y;
				hit.posZ = z1 + t * vect.z;
				if (pointVsBox(hit.posX, hit.posY, hit.posZ, xmax, ymax, zmax, xmin, ymin, zmin ))
				{
					// 0 0 0
					hit.qVector.w = 1;
					hit.qVector.x = 0;
					hit.qVector.y = 0;
					hit.qVector.z = 0;
					hit.distance = t;
					//hit.distance = sqrt(   pow((hit.posX - x1),2) +  pow((hit.posY - y1),2)  + pow((hit.posZ - z1),2) );  //distance in units
					vCollisions.push_back(hit);
				}

		t = (ymin - y1) / vect.y;
				hit.posX = x1 + t * vect.x;
				hit.posY = y1 + t * vect.y;
				hit.posZ = z1 + t * vect.z;
				if (pointVsBox(hit.posX, hit.posY, hit.posZ, xmax, ymax, zmax, xmin, ymin, zmin ))
				{
					// 180 0 0
					hit.qVector.w = 2.267948967e-007;
					hit.qVector.x = 1;
					hit.qVector.y = 0;
					hit.qVector.z = 0;
					hit.distance = t;
					//hit.distance = sqrt(   pow((hit.posX - x1),2) +  pow((hit.posY - y1),2)  + pow((hit.posZ - z1),2) );  //distance in units
					vCollisions.push_back(hit);
				}
	}

	if (vect.z != 0)  //protect from divide by 0
	{
	t = (zmax - z1) / vect.z;
			hit.posX = x1 + t * vect.x;
			hit.posY = y1 + t * vect.y;
			hit.posZ = z1 + t * vect.z;
			if (pointVsBox(hit.posX, hit.posY, hit.posZ, xmax, ymax, zmax, xmin, ymin, zmin ))
			{
				// 90 0 0
				hit.qVector.w = 0.7071068614;
				hit.qVector.x = 0.707106701;
				hit.qVector.y = 0;
				hit.qVector.z = 0;
				hit.distance = t;
				//hit.distance = sqrt(   pow((hit.posX - x1),2) +  pow((hit.posY - y1),2)  + pow((hit.posZ - z1),2) );  //distance in units
				vCollisions.push_back(hit);
			}

	t = (zmin - z1) / vect.z;
			hit.posX = x1 + t * vect.x;
			hit.posY = y1 + t * vect.y;
			hit.posZ = z1 + t * vect.z;
			if (pointVsBox(hit.posX, hit.posY, hit.posZ, xmax, ymax, zmax, xmin, ymin, zmin ))
			{
				// -90 0 0
				hit.qVector.w = 0.7071068614;
				hit.qVector.x = -0.707106701;
				hit.qVector.y = 0;
				hit.qVector.z = 0;
				hit.distance = t;
				//hit.distance = sqrt(   pow((hit.posX - x1),2) +  pow((hit.posY - y1),2)  + pow((hit.posZ - z1),2) );  //distance in units
				vCollisions.push_back(hit);
			}
	}

	//still must add box name and vars after the return, as only the vectors and coords have been added
	return vCollisions;
}

bool collisionManager::oneDimentionalTest( float &min1, float &max1,  float &min2, float &max2)
{

	if ((min1 >= min2) && (min1 <= max2))
		return 1;
	if ((max1 >= min2) && (max1 <= max2))
		return 1;
	if ((min2 >= min1) && (min2 <= max1))
		return 1;
	if ((max2 >= min1) && (max2 <= max1))
		return 1;

	return 0;
}


collisionBox collisionManager::rayToBox(float &x1, float &y1, float &z1, float &x2, float &y2, float &z2)
{
	collisionBox cBox;

	if (x1 > x2)
	{
		cBox.xmax = x1;
		cBox.xmin = x2;
	}
	else
	{
		cBox.xmax = x2;
		cBox.xmin = x1;
	}

	if (y1 > y2)
	{
		cBox.ymax = y1;
		cBox.ymin = y2;
	}
	else
	{
		cBox.ymax = y2;
		cBox.ymin = y1;
	}

	if (z1 > z2)
	{
		cBox.zmax = z1;
		cBox.zmin = z2;
	}
	else
	{
		cBox.zmax = z2;
		cBox.zmin = z1;
	}

	return cBox;
}


void collisionManager::eulerToQuat(float &roll, float &pitch, float &yaw, Ogre::Quaternion * quat)
{//this procedure is code from gamasutra article: http://www.gamasutra.com/features/19980703/quaternions_01.htm by Nick Bobick
	float cr, cp, cy, sr, sp, sy, cpcy, spsy;

	// calculate trig identities
	cr = cos(roll/2);
	cp = cos(pitch/2);
	cy = cos(yaw/2);

	sr = sin(roll/2);
	sp = sin(pitch/2);
	sy = sin(yaw/2);
	
	cpcy = cp * cy;
	spsy = sp * sy;

	quat->w = cr * cpcy + sr * spsy;
	quat->x = sr * cpcy - cr * spsy;
	quat->y = cr * sp * cy + sr * cp * sy;
	quat->z = cr * cp * sy - sr * sp * cy;
}

void collisionManager::orderCollisionHitsByDistance(std::vector<collisionHit> *hits)
{
	//if we are using t as distance, this will only work properly when comparing hits generated from the same line calculation.	
	if (hits->size())
	{
		std::vector<collisionHit> hitsTemp = (*hits);	
		hits->clear();
		collisionHit largest;		
		int lastLargestIterator;
		
		int arraySize = hitsTemp.size();
		for (int i = 0; i < arraySize ; i ++)
		{
			largest.distance = 0;
			for (int j = 0; j < arraySize ; j ++)
			{
				if ((hitsTemp[j].distance >= largest.distance) &&
					(hitsTemp[j].distance >= 0))
				{
					largest = hitsTemp[j];
					lastLargestIterator = j;
				}
			}
			(*hits).push_back(largest);
			hitsTemp[lastLargestIterator].distance = -1;
		}
	}
}


string collisionManager::getAttrib(TiXmlElement *XMLNode, const string &attrib, const string &defaultValue)
{
	if(XMLNode->Attribute(attrib.c_str()))
		return XMLNode->Attribute(attrib.c_str());
	else
		return defaultValue;
}

Real collisionManager::getAttribReal(TiXmlElement *XMLNode, const string &attrib, Real defaultValue)
{
	if(XMLNode->Attribute(attrib.c_str()))
		return StringConverter::parseReal(XMLNode->Attribute(attrib.c_str()));
	else
		return defaultValue;
}


/*

//distance
 a^2 = x^2 + y^2 + z^2

*/