#include "SimpleVehicle.h"


// Ogre Matrix inverse really sucks (it use a full Gaussian pivoting when a simple transpose follow by vector rotation will do.
inline Ogre::Matrix4 MatrixInverse (const Ogre::Matrix4& matrix)
{
	Ogre::Matrix4 tmp (matrix.transpose());
	Ogre::Vector4 posit (tmp[3][0], tmp[3][1], tmp[3][2], 1.0f);
	tmp[3][0] = 0.0f;
	tmp[3][1] = 0.0f;
	tmp[3][2] = 0.0f;

	posit = tmp * posit;
	tmp[0][3] = -posit.x;
	tmp[1][3] = -posit.y;
	tmp[2][3] = -posit.z;
	return tmp;
}


int SimpleVehicle::mEntityCount = 0;

SimpleVehicle* SimpleVehicle::create (CAR_CONFIG* config, Ogre::SceneManager* mgr, OgreNewt::World* world)
{
	SimpleVehicle* car;
	Ogre::Entity* carEnt;
	OgreNewt::Body* carBody;
	Ogre::SceneNode* parentNode;

	// create the main car body
	carEnt = mgr->createEntity( "CarEntity" + Ogre::StringConverter::toString(mEntityCount++), config->m_name);
	parentNode = mgr->getRootSceneNode()->createChildSceneNode();
	parentNode->attachObject (carEnt);


	OgreNewt::ConvexCollisionPtr col (new OgreNewt::CollisionPrimitives::ConvexHull (world, carEnt, 0));
	carBody = new OgreNewt::Body (world, col);

	// base mass on the size of the object.
	Ogre::Real mass = 100.0f;
	// calculate the inertia based on box formula and mass
	Ogre::Vector3 inertia, offset;
	col->calculateInertialMatrix(inertia, offset);

	carBody->attachNode (parentNode);
	carBody->setMassMatrix (mass, mass*inertia);
	carBody->setCenterOfMass (offset);
	carBody->setStandardForceCallback();

	// create the car joint;
	car = new SimpleVehicle (carBody, 4);


	// create and attach all tires
	CAR_CONFIG::TIRE* tireGeo = &config->front_right;
	for (int i = 0; i < 4; i ++) {
		Ogre::Entity* tireEnt;
		Ogre::SceneNode* tireNode;
		Ogre::Node* tireHarpointAttachement;
		

		// attach the tire entity as a child node of the body node
		tireEnt = mgr->createEntity( "CarEntity" + Ogre::StringConverter::toString(mEntityCount++), tireGeo[i].meshName);
		Ogre::Vector3 posit (tireGeo[i].m_posit[0], tireGeo[i].m_posit[1], tireGeo[i].m_posit[2]);
		Ogre::Quaternion rotation (tireGeo[i].m_rotation[0], tireGeo[i].m_rotation[1], tireGeo[i].m_rotation[2], tireGeo[i].m_rotation[3]);
		
		// create the tire hard point attachment
		tireHarpointAttachement = parentNode->createChild (posit, Ogre::Quaternion::IDENTITY);

		// attach the tire to the hard point node
		tireNode = (Ogre::SceneNode*) tireHarpointAttachement->createChild (Ogre::Vector3::ZERO, rotation);

		// attach the mesh entity to the tire node
		tireNode->attachObject (tireEnt);

		// calculate the tire dimension from the entity AABB
		Ogre::AxisAlignedBox tireBox (tireEnt->getBoundingBox());
		Ogre::Real width = tireBox.getMaximum().z - tireBox.getMinimum().z; 
		Ogre::Real radius = 0.5f * (tireBox.getMaximum().x - tireBox.getMinimum().x); 

// for now just to get it going
Ogre::Real mass = 10.0f;
Ogre::Real friction = 0.1f;
Ogre::Real suspensionLenght = 0.2f;
Ogre::Real susSpring = 100.0f;
Ogre::Real susShock = 0.0f;
		car->AddSingleSuspensionTire (tireHarpointAttachement, posit, mass, radius, width, friction, suspensionLenght, susSpring, susShock);
	}


	return  car;
}

// SimpleVehicle constructor.  this creates and sets up the entire vehicle!
SimpleVehicle::SimpleVehicle(OgreNewt::Body* carBody, int maxTiresCount)
	:RayCastVehicle(carBody, maxTiresCount)
{
}

SimpleVehicle::~SimpleVehicle(void)
{
}


void SimpleVehicle::setTireTransform (void* tireNodePtr, const Ogre::Matrix4& tireMatrix) const
{
	// cats the user data pointer t0 a scene node;
	Ogre::Node* tireNode = (Ogre::Node*) tireNodePtr;
	Ogre::Node* parentNode = tireNode->getParent();	

	const Ogre::Matrix4& parentMatrix = parentNode->_getFullTransform();

	// Ogre Matrix inverse really sucks (it use a full Gaussian pivoting when a simple transpose follow by vector rotation will do.
	const Ogre::Matrix4 tireLocalMatrix ( MatrixInverse(parentMatrix) * tireMatrix);

	tireNode->setOrientation(tireLocalMatrix.extractQuaternion());
	tireNode->setPosition(Ogre::Vector3 (tireLocalMatrix[0][3], tireLocalMatrix[1][3], tireLocalMatrix[2][3]));
}