#include "PhysicsGlobal.h"
#include "PhysicsGlobal.h"

#include "CPhysics.h"

using namespace std; // Visual Studio 2005

/// Static object of irrlicht device.
//! This is needed in static callbacks.
IrrlichtDevice* CPhysics::m_device;

/// Constructor
CPhysics::CPhysics()
{

}

/// Destructor
CPhysics::~CPhysics()
{

}

/// Initialisation function.
//! This should be called before anything else, as it initialises Newton. The ITimer* you pass should be the one from
//! the Irrlicht device you are using - this allows IPhysics to update automatically.
void CPhysics::init(IrrlichtDevice* dev)
{
	m_device = dev;

//	m_log = new CLog;
//	m_log->doLog(true, true, "IPhysics.log");

	setSolverModel(ESM_FAST);
	m_isCam = false;
	m_showDebug = false;

	f32 min[3];
	min[0] = -1000.0f;
	min[1] = -1000.0f;
	min[2] = -1000.0f;
	f32 max[3];
	max[0] = 1000.0f;
	max[1] = 1000.0f;
	max[2] = 1000.0f;
	NewtonSetWorldSize(m_world, &min[0], &max[0]); //default nice and big

	m_materialManager = new CMaterialManager;
	m_materialManager->init(m_world);

	// these numbers from the Newton docs - can be user set later
	int i = m_materialManager->getDefaultMaterial();
	m_materialManager->setFriction(i,i,0.8f,0.4f);
	m_materialManager->setElasticity(i,i,0.3f);
	m_materialManager->setSoftness(i,i,0.05f);

	m_timestepAccumulator = 0;
	m_timer = m_device->getTimer();
	m_oldTime = 0;

	m_updateMode = UM_FIXED_FRAME_RATE;
	m_updatesPerSecond = 60; // default to 60 fps
}

void CPhysics::doLog(bool doLog, bool doEcho, bool path)
{
//	m_log->doLog(doLog, doEcho, path);
}

/// Sets the update mode.
//! The update mode determines how Newton is updated. The current choices are UM_FIXED_FRAME_RATE, which ensures that
//! Newton is kept at a constant framerate (default: 60), and UM_EVERY_FRAME, which updates Newton every frame using the
//! correct timestep for the interval since. Both modes update dependant on time, not the frame rate of your application.
//! UM_EVERY_FRAME is a little smoother, but a little more computationally expensive.
void CPhysics::setUpdateMode(PHYSICS_UPDATE_MODE updateMode)
{
	m_updateMode = updateMode;
}

/// Sets the number of updates per second.
//! This value will only be used if the update mode is set to UM_FIXED_FRAME_RATE. The default value for this is 60.
void CPhysics::setUpdatesPerSecond(s32 updatesPerSecond)
{
	m_updatesPerSecond = updatesPerSecond;
}

/// The update function.
//! This should be called every frame, regardless of which update mode you are using.
void CPhysics::update()
{
	f32 time = (f32)m_timer->getTime();
	f32 timestep_ms = time - m_oldTime;
	m_oldTime = time;

	update(timestep_ms);
}

/// The update function.
//! This update function allows you to pass your own timestep. This is only intended for effects like slow-motion, if you
//! want to control how fast time progresses, or else if you need to control how the timestep is calculated (for example,
//! maybe you want to average the timesteps if your framerate varies a lot). If you just want IPhysics to run 'normally',
//! use the simpler version which takes no arguments.
void CPhysics::update(f32 timestep_ms)
{
	if (m_isCam)
		m_camEntity->update();

	// cap at 50ms
	if(timestep_ms > 50.0f)
	{
		timestep_ms = 50.0f;
	}

	m_timestepAccumulator += timestep_ms;
	f32 newtonTimestep = 1000.0f / m_updatesPerSecond; // 1 second / fps

	if(m_updateMode == UM_FIXED_FRAME_RATE)
	{
		while(m_timestepAccumulator >= newtonTimestep) // ensure physics runs at desired framerate
		{
			NewtonUpdate(m_world, newtonTimestep * 0.001f); // need to convert to seconds
			m_timestepAccumulator -= newtonTimestep;
		}
	}
	else if(m_updateMode == UM_EVERY_FRAME)
	{
		NewtonUpdate(m_world, timestep_ms * 0.001f); // need to convert to seconds
	}

	if (m_showDebug)
		NewtonWorldForEachBodyDo (m_world, NewtonDebugBody);

}

/// Cleanup function.
//! Call this to close down IPhysics. It is important to call this, as IPhysics does use some dynamically allocated memory,
//! and failing to call this would leave it inilialised and it will leak.
void CPhysics::close()
{
//	cout << "3.1" << endl;
	// used new to create entites - need to call delete for them
	list<IPhysicsBaseEntity*>::Iterator nextEntity;

	for(nextEntity = m_entityList.begin(); nextEntity != m_entityList.end(); ++nextEntity)
	{
			delete(*nextEntity);
	}

//TODO - this is a problem at the moment
//	delete[] m_materialManager;


//	delete[] m_log;
}


/// Adds a car.
//! Creates a car, based on the supplied data. Note that the car implementation is currently somewhat primitive; this will
//! be extended in future versions.
IPhysicsCar* CPhysics::addCar(SPhysicsCar* car)
{
	CPhysicsCar* newCar = new CPhysicsCar();
	newCar->assembleCarPhysics(car, m_world);
	m_entityList.push_front(newCar);

	return newCar;
}

/// Adds a static mesh.
//! A static mesh is any arbitrary mesh that should never move when things collide with it, and that shouldn't be affected by
//! gravity. Use this for things like rooms and levels. This supports all three Irrlicht vertex types, so lightmapped meshes
//! and suchlike should all work fine.
IPhysicsEntity* CPhysics::addEntity(SPhysicsStaticMesh* staticMesh)
{
	int i,j;

	IMeshBuffer *mb;
	u16* mb_indices;
	vector3df vArray[3]; // vertex array (3*3 floats)

    NewtonCollision *treeCollision;
	treeCollision = NewtonCreateTreeCollision(m_world, NULL);
	NewtonTreeCollisionBeginBuild(treeCollision);

	vector3df meshScale = staticMesh->scale;
	IMesh *mesh = staticMesh->mesh->getMesh(0);

	for (i = 0; i < mesh->getMeshBufferCount(); i ++)
	{
		mb = mesh->getMeshBuffer(i);

		mb_indices  = mb->getIndices();

		// need to switch between the vertex types, otherwise getVertices() will crash
		if (mb->getVertexType() == EVT_STANDARD)
		{
			S3DVertex* mb_vertices = (S3DVertex*)mb->getVertices();

			// add each triangle from the mesh
			for (j=0; j<mb->getIndexCount(); j+=3)
			{
				vArray[0] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
				vArray[1] = mb_vertices[mb_indices[j+1]].Pos * IrrToNewton * meshScale.Y;
				vArray[2] = mb_vertices[mb_indices[j+2]].Pos * IrrToNewton * meshScale.Z;

				// make sure we do not add degenerated polygons to the tree
				vector3df e0 (vArray[1] - vArray[0]);
				vector3df e1 (vArray[2] - vArray[0]);

				// the area of the face is the cross product
				vector3df area (e0.crossProduct (e1));

				// skip faces with very small area
				f32 mag = area.dotProduct (area);
				if (mag > 1.0e-6f)
					NewtonTreeCollisionAddFace(treeCollision, 3, &vArray[0].X, sizeof (vector3df), 1);
			}
		}
		else if (mb->getVertexType() == EVT_2TCOORDS)
		{
			S3DVertex2TCoords* mb_vertices = (S3DVertex2TCoords*)mb->getVertices();

			// add each triangle from the mesh
			for (j=0; j<mb->getIndexCount(); j+=3)
			{
				vArray[0] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
				vArray[1] = mb_vertices[mb_indices[j+1]].Pos * IrrToNewton * meshScale.Y;
				vArray[2] = mb_vertices[mb_indices[j+2]].Pos * IrrToNewton * meshScale.Z;

				// make sure we do not add degenerated polygons to the tree
				vector3df e0 (vArray[1] - vArray[0]);
				vector3df e1 (vArray[2] - vArray[0]);

				// the area of the face is the cross product
				vector3df area (e0.crossProduct (e1));

				// skip faces with very small area
				f32 mag = area.dotProduct (area);
				if (mag > 1.0e-6f)
					NewtonTreeCollisionAddFace(treeCollision, 3, &vArray[0].X, sizeof (vector3df), 1);
			}
		}
		else if(mb->getVertexType() == EVT_TANGENTS)
		{
			S3DVertexTangents* mb_vertices = (S3DVertexTangents*)mb->getVertices();

			// add each triangle from the mesh
			for (j=0; j<mb->getIndexCount(); j+=3)
			{
				vArray[0] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
				vArray[1] = mb_vertices[mb_indices[j+1]].Pos * IrrToNewton * meshScale.Y;
				vArray[2] = mb_vertices[mb_indices[j+2]].Pos * IrrToNewton * meshScale.Z;

				// make sure we do not add degenerated polygons to the tree
				vector3df e0 (vArray[1] - vArray[0]);
				vector3df e1 (vArray[2] - vArray[0]);

				// the area of the face is the cross product
				vector3df area (e0.crossProduct (e1));

				// skip faces with very small area
				f32 mag = area.dotProduct (area);
				if (mag > 1.0e-6f)
					NewtonTreeCollisionAddFace(treeCollision, 3, &vArray[0].X, sizeof (vector3df), 1);
			}
		}
	}

	NewtonTreeCollisionEndBuild(treeCollision, 1);

	// create a rigid body
	NewtonBody* treeBody;
	treeBody = NewtonCreateBody(m_world, treeCollision);

	// release the collsion tree
	NewtonReleaseCollision (m_world, treeCollision);

	staticMesh->node->setScale(staticMesh->scale);

	CPhysicsEntity* entity = new CPhysicsEntity();
	entity->init(m_world, treeBody, staticMesh->node, m_log);
	NewtonBodySetUserData(treeBody, entity);
	m_entityList.push_front(entity);

	return entity;
}

/*
void CPhysics::addMeshToTreeCollisionStandard(IMeshBuffer* meshBuffer, NewtonCollision* treeCollision, vector3df meshScale)
{
	vector3df vArray[3]; // vertex array (3*3 floats)
	int j;

	S3DVertex* mb_vertices = (S3DVertex*)meshBuffer->getVertices();

	u16* mb_indices  = meshBuffer->getIndices();

	// add each triangle from the mesh
	for (j=0; j<meshBuffer->getIndexCount(); j+=3)
	{
		vArray[0] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
		vArray[1] = mb_vertices[mb_indices[j+1]].Pos * IrrToNewton * meshScale.Y;
		vArray[2] = mb_vertices[mb_indices[j+2]].Pos * IrrToNewton * meshScale.Z;

		// make sure we do not add degenerated polygons to the tree
		vector3df e0 (vArray[1] - vArray[0]);
		vector3df e1 (vArray[2] - vArray[0]);

		// the area of the face is the cross product
		vector3df area (e0.crossProduct (e1));

		// skip faces with very small area
		f32 mag = area.dotProduct (area);
		if (mag > 1.0e-6f)
			NewtonTreeCollisionAddFace(treeCollision, 3, &vArray[0].X, sizeof (vector3df), 1);
	}
}

void CPhysics::addMeshToTreeCollision2TCoords(IMeshBuffer* meshBuffer, NewtonCollision* treeCollision, vector3df meshScale)
{
	vector3df vArray[3]; // vertex array (3*3 floats)
	int j;

	S3DVertex2TCoords* mb_vertices = (S3DVertex2TCoords*)meshBuffer->getVertices();

	u16* mb_indices  = meshBuffer->getIndices();

	// add each triangle from the mesh
	for (j=0; j<meshBuffer->getIndexCount(); j+=3)
	{
		vArray[0] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
		vArray[1] = mb_vertices[mb_indices[j+1]].Pos * IrrToNewton * meshScale.Y;
		vArray[2] = mb_vertices[mb_indices[j+2]].Pos * IrrToNewton * meshScale.Z;

		// make sure we do not add degenerated polygons to the tree
		vector3df e0 (vArray[1] - vArray[0]);
		vector3df e1 (vArray[2] - vArray[0]);

		// the area of the face is the cross product
		vector3df area (e0.crossProduct (e1));

		// skip faces with very small area
		f32 mag = area.dotProduct (area);
		if (mag > 1.0e-6f)
			NewtonTreeCollisionAddFace(treeCollision, 3, &vArray[0].X, sizeof (vector3df), 1);
	}
}

void CPhysics::addMeshToTreeCollisionTangents(IMeshBuffer* meshBuffer, NewtonCollision* treeCollision, vector3df meshScale)
{
	vector3df vArray[3]; // vertex array (3*3 floats)
	int j;

	S3DVertexTangents* mb_vertices = (S3DVertexTangents*)meshBuffer->getVertices();

	u16* mb_indices  = meshBuffer->getIndices();

	// add each triangle from the mesh
	for (j=0; j<meshBuffer->getIndexCount(); j+=3)
	{
		vArray[0] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
		vArray[1] = mb_vertices[mb_indices[j+1]].Pos * IrrToNewton * meshScale.Y;
		vArray[2] = mb_vertices[mb_indices[j+2]].Pos * IrrToNewton * meshScale.Z;

		// make sure we do not add degenerated polygons to the tree
		vector3df e0 (vArray[1] - vArray[0]);
		vector3df e1 (vArray[2] - vArray[0]);

		// the area of the face is the cross product
		vector3df area (e0.crossProduct (e1));

		// skip faces with very small area
		f32 mag = area.dotProduct (area);
		if (mag > 1.0e-6f)
			NewtonTreeCollisionAddFace(treeCollision, 3, &vArray[0].X, sizeof (vector3df), 1);
	}
}*/

/// Adds terrain.
//! This function now contains a call to set the terrainSceneNode's scale to the scale provided in the struct, so you don't
//! need to. It is important though to ensure that the terrainScale is set - if you want the terrain at the original size,
//! you still need to set the terrainScale to (1, 1, 1). If you don't, the terrain will be invisible (as the scale will be
//! (0, 0, 0)).
IPhysicsEntity* CPhysics::addEntity(SPhysicsTerrain* terrain)
{
	terrain->node->setScale(terrain->scale);

	matrix4 offset;
	offset.makeIdentity();

	SMeshBufferLightMap meshBuffer;

	terrain->node->getMeshBufferForLOD(meshBuffer, 2);

	meshBuffer.Indices.set_free_when_destroyed(false);
	meshBuffer.Vertices.set_free_when_destroyed(false);

	S3DVertex2TCoords *vertices = (S3DVertex2TCoords*)meshBuffer.getVertices();
	u16 *indices = meshBuffer.getIndices();

	NewtonCollision* treeCollision = NewtonCreateTreeCollision(m_world, 0);
	NewtonTreeCollisionBeginBuild(treeCollision);

	s32 v1i, v2i, v3i;
	f32 vArray[9];

	vector3df tScale = terrain->scale;
	for (int i = 0; i < meshBuffer.getIndexCount(); i +=3)
	{

		v1i = indices[i];
		v2i = indices[i+1];
		v3i = indices[i+2];

		vArray[0] = (vertices[v1i].Pos.X * IrrToNewton * tScale.X);
		vArray[1] = (vertices[v1i].Pos.Y * IrrToNewton * tScale.Y);
		vArray[2] = (vertices[v1i].Pos.Z * IrrToNewton * tScale.Z);
		vArray[3] = (vertices[v2i].Pos.X * IrrToNewton * tScale.X);
		vArray[4] = (vertices[v2i].Pos.Y * IrrToNewton * tScale.Y);
		vArray[5] = (vertices[v2i].Pos.Z * IrrToNewton * tScale.Z);
		vArray[6] = (vertices[v3i].Pos.X * IrrToNewton * tScale.X);
		vArray[7] = (vertices[v3i].Pos.Y * IrrToNewton * tScale.Y);
		vArray[8] = (vertices[v3i].Pos.Z * IrrToNewton * tScale.Z);


		NewtonTreeCollisionAddFace(treeCollision, 3, (float*)vArray, 12, 11);
	}

	NewtonTreeCollisionEndBuild(treeCollision, 0);

	NewtonBody* body = NewtonCreateBody(m_world, treeCollision);

	CPhysicsEntity* entity = new CPhysicsEntity();

	entity->init(m_world, body, terrain->node, m_log);

	entity->setScale(terrain->scale);

	NewtonBodySetUserData(body, entity);

	m_entityList.push_front(entity);

	return entity;
}


/// Adds a Primitve
//! Note that it is best to use primitives wherever you can, as they are much faster than using a convex hull.
IPhysicsEntity* CPhysics::addEntity(SPhysicsPrimitive* primitive)
{
	NewtonCollision* collision;

	if (!primitive->node)
	{
//		m_log->writeLog("Warning, no node found");
	}

	if (primitive->node)
		primitive->node->setScale(primitive->scale);

	vector3df size = primitive->size * primitive->scale * IrrToNewton;
	f32 radius = primitive->radius * primitive->scale.X * IrrToNewton;
	f32 height = primitive->height * primitive->scale.Y * IrrToNewton;

	if (primitive->primitiveType == EPT_NULL)
	{
		collision = NewtonCreateNull(m_world);
	}
	else if (primitive->primitiveType == EPT_CUBE)
	{
		collision = NewtonCreateBox(m_world, size.X, size.Y, size.Z, primitive->offset.pointer());
	}
	else if (primitive->primitiveType == EPT_SPHERE)
	{
		collision = NewtonCreateSphere(m_world, size.X, size.Y, size.Z, primitive->offset.pointer());
	}
	else if (primitive->primitiveType == EPT_CONE)
	{
		primitive->offset.setRotationDegrees(primitive->offset.getRotationDegrees() + vector3df(0,0,90));
		collision = NewtonCreateCone(m_world, radius, height, primitive->offset.pointer());
		size.X = radius;
		size.Y = height;
	}
	else if (primitive->primitiveType == EPT_CAPSULE)
	{
		collision = NewtonCreateCapsule(m_world, radius, height, primitive->offset.pointer());
		size.X = radius;
		size.Y = height;
	}
	else if (primitive->primitiveType == EPT_CYLINDER)
	{
		collision = NewtonCreateCylinder(m_world, radius, height, primitive->offset.pointer());
		size.X = radius;
		size.Y = height;
	}
	else if (primitive->primitiveType == EPT_CHAMFERCYLINDER)
	{
		collision = NewtonCreateChamferCylinder(m_world, radius, height, primitive->offset.pointer());
		size.X = radius;
		size.Y = height;
	}

	NewtonBody* body = NewtonCreateBody(m_world, collision);

	NewtonBodySetUserData(body, &size);

	return setUpRigidBody(body, collision, primitive);
}

/// Adds a user Mesh
//! This function will create a entity of a user created mesh.
IPhysicsEntity* CPhysics::addEntity(SPhysicsMesh* mesh)
{
	mesh->node->setScale(mesh->scale);

	IMeshBuffer *mb;
	int cnt, i, j;
	cnt = -1;
	u16* mb_indices;
	vector3df convexHull[65535];

	IMesh* msh = mesh->mesh->getMesh(mesh->frame);

	vector3df meshScale = mesh->scale;

	if (msh->getMeshBufferCount() < 1)
	{
//		m_log->writeLog("No mesh buffer found");;
		return NULL;
	}

	for (i = 0; i < msh->getMeshBufferCount(); i ++)
	{
		mb = msh->getMeshBuffer(i);
		mb_indices  = mb->getIndices();

		if (mb->getVertexType() == EVT_STANDARD)
		{
			S3DVertex* mb_vertices = (S3DVertex*)mb->getVertices();

			// add each triangle from the mesh
			for (j=0; j<mb->getIndexCount(); j++)
			{
				cnt ++;
				convexHull[cnt] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
			}
		}
		else if(mb->getVertexType() == EVT_2TCOORDS)
		{
			S3DVertex2TCoords* mb_vertices = (S3DVertex2TCoords*)mb->getVertices();

			// add each triangle from the mesh
			for (j=0; j<mb->getIndexCount(); j++)
			{
				cnt ++;
				convexHull[cnt] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
			}
		}
		else if(mb->getVertexType() == EVT_TANGENTS)
		{
			S3DVertexTangents* mb_vertices = (S3DVertexTangents*)mb->getVertices();

			// add each triangle from the mesh
			for (j=0; j<mb->getIndexCount(); j++)
			{
				cnt ++;
				convexHull[cnt] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
			}
		}
	}

	NewtonCollision* meshCollision = NewtonCreateConvexHull(m_world,cnt,&convexHull[0].X,sizeof(vector3df),NULL);

	// create a rigid body
	NewtonBody* body = NewtonCreateBody(m_world, meshCollision);

	return setUpRigidBody(body, meshCollision, mesh);
}

/*
void CPhysics::addCollisionStandardToMesh(IMeshBuffer* mb, vector3df meshScale)
{
	S3DVertex* mb_vertices = (S3DVertex*)mb->getVertices();

	u16* mb_indices  = mb->getIndices();

	// add each triangle from the mesh
	for (int j=0; j<mb->getIndexCount(); j++)
	{
		cnt ++;
		convexHull[cnt] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
	}
}

void CPhysics::addCollision2TCoordsToMesh(IMeshBuffer* mb, vector3df meshScale)
{
	S3DVertex2TCoords* mb_vertices = (S3DVertex2TCoords*)mb->getVertices();

	u16* mb_indices  = mb->getIndices();

	// add each triangle from the mesh
	for (int j=0; j<mb->getIndexCount(); j++)
	{
		cnt ++;
		convexHull[cnt] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
	}
}

void CPhysics::addCollisionTangentsToMesh(IMeshBuffer* mb, vector3df meshScale)
{
	S3DVertexTangents* mb_vertices = (S3DVertexTangents*)mb->getVertices();

	u16* mb_indices  = mb->getIndices();

	// add each triangle from the mesh
	for (int j=0; j<mb->getIndexCount(); j++)
	{
		cnt ++;
		convexHull[cnt] = mb_vertices[mb_indices[j]].Pos * IrrToNewton * meshScale.X;
	}
}*/

IPhysicsEntity* CPhysics::addEntity(SPhysicsSerializedEntity* mesh)
{
	FILE* handle=fopen(mesh->path.c_str(),"rb");
	if(handle==NULL) {
//		m_log->writeLog("Unable to open serialized mesh.");
		return false;
	}

	NewtonCollision* collision = NewtonCreateTreeCollisionFromSerialization(
		m_world,
		NULL,
		getFromFile,
		handle
		);
	fclose(handle);

	NewtonBody* body = NewtonCreateBody(m_world, collision);

	mesh->node->setScale(mesh->scale);

	return setUpRigidBody(body, collision, mesh);
}

void CPhysics::getFromFile(void* serializeHandle, void* buffer, size_t size)
{
	fread (buffer, size, 1, (FILE*) serializeHandle);
}

// apply generic setup for all rigid bodies: mass matrix, transform and forceandtorque callbacks
// overloaded addEntity functions are for creating the different hull shapes available (cylinder, sphere etc)
IPhysicsEntity* CPhysics::setUpRigidBody(NewtonBody *body, NewtonCollision* collision, SPhysicsAttributes* attributes)
{
	NewtonBodySetTransformCallback(body, physicsSetTransform);

	f32 mass = 0;
	if(attributes->bodyType == EBT_DYNAMIC)
	{
		vector3df inertia;
		vector3df origin(0,0,0);

		NewtonBodySetForceAndTorqueCallback(body, applyForceAndTorqueEvent);
		mass = attributes->mass;

		NewtonConvexCollisionCalculateInertialMatrix(collision, &inertia.X, &origin.X);

		NewtonBodySetCentreOfMass(body, &origin.X);

		NewtonBodySetMassMatrix (body, mass, inertia.X, inertia.Y, inertia.Z);
	}

	NewtonReleaseCollision (m_world, collision);

	CPhysicsEntity* entity = new CPhysicsEntity();

	entity->init(m_world, body, attributes->node, m_log);

//	entity->setRotation(vector3df(0,0,0));

	NewtonBodySetUserData(body, entity);

	m_entityList.push_front(entity);

	return entity;
}


CPhysicsCamera* CPhysics::addCamera(SPhysicsCamera* camera)
{
	if (!camera->node)
	{
		//m_log->writeLog("No camera node found while creating camera entity!");
		return NULL;
	}

	int charMat = m_materialManager->createMaterial();
	m_materialManager->setFriction(charMat,charMat,2.0f,1.0f);
	m_materialManager->setElasticity(charMat, charMat, 0);
	m_materialManager->setSoftness(charMat, charMat, 0);

	m_camEntity = new CPhysicsCamera;

	m_camEntity->init(m_device, m_world, camera);
	m_camEntity->setMaterial(charMat);

	m_isCam = true;

	return m_camEntity;
}

IPhysicsEntity* CPhysics::addEntity(IPhysicsBaseEntity* first,  ...)
{
//	cout << "A" << endl;
	array<NewtonCollision*> compound;
	IPhysicsBaseEntity* nNext;

	int nCount   = 0;
	f32 mass = 0;

	va_list marker;

	va_start( marker, first ); // Initialize variable arguments.

	nNext = first;

	//NewtonBodyGetMassMatrix(first->getBody(), &mass, NULL, NULL, NULL);

//	cout << "B" << endl;

	while( nNext != NULL )
	{
		if (nNext->getBody())
//			cout << "GOT IT " << endl;

		compound.push_back(NewtonBodyGetCollision(nNext->getBody()));

//		cout << nCount << endl;
		++nCount;
		nNext = va_arg( marker, IPhysicsBaseEntity* );
		//f32 tempMass;
		//NewtonBodyGetMassMatrix(nNext->getBody(), &tempMass, NULL, NULL, NULL);
		//mass += tempMass;
	}
//	cout << "C" << endl;

	va_end( marker ); // Reset variable arguments.

	NewtonCollision* collision = NewtonCreateCompoundCollision(m_world, nCount, compound.pointer());

	NewtonBody* body = NewtonCreateBody(m_world, collision);

	SPhysicsAttributes attributes;
	attributes.mass = 10;
	attributes.bodyType = EBT_DYNAMIC;

//	cout << "D" << endl;

	return setUpRigidBody(body, collision, &attributes);
}

CPhysicsHinge* CPhysics::addHingeJoint(SPhysicsHingeJoint* joint)
{
	CPhysicsHinge* hinge = new CPhysicsHinge;

	hinge->init(m_world, joint);

	return hinge;
}

CPhysicsBallSocket* CPhysics::addBallSocketJoint(SPhysicsBallSocketJoint* joint)
{
	CPhysicsBallSocket* ballSocket = new CPhysicsBallSocket;

	ballSocket->init(m_world, joint);

	return ballSocket;
}

CPhysicsSlider* CPhysics::addSliderJoint(SPhysicsSliderJoint* joint)
{
	CPhysicsSlider* slider = new CPhysicsSlider;

	slider->init(m_world, joint);

	return slider;
}

CPhysicsCorkscrew* CPhysics::addCorkscrewJoint(SPhysicsCorkscrewJoint* joint)
{
	CPhysicsCorkscrew* corkscrew = new CPhysicsCorkscrew;

	corkscrew->init(m_world, joint);

	return corkscrew;
}

CPhysicsUniversal* CPhysics::addUniversalJoint(SPhysicsUniversalJoint* joint)
{
	CPhysicsUniversal* universal = new CPhysicsUniversal;

	universal->init(m_world, joint);

	return universal;
}

CPhysicsUpVector* CPhysics::addUpVectorJoint(SPhysicsUpVector* upVector)
{
	CPhysicsUpVector* uVector = new CPhysicsUpVector;

	uVector->init(m_world, upVector);

	return uVector;
}

CPhysicsPulley* CPhysics::addPulley(SPhysicsPulley* pulley)
{
	CPhysicsPulley* pulleyJoint = new CPhysicsPulley;

	pulleyJoint->init(m_world, pulley);

	return pulleyJoint;
}

/// Drops a test cube.
//! This is for testing and debugging - a test cube is created using the built-in Irrlicht primitive and dropped from the
//! specified position. The ISceneManager pointer should be the one from the IrrlichtDevice that you are using. The size of
//! the cube will be 10*10*10 and the mass will be 10.
IPhysicsEntity* CPhysics::throwTestCube(CPhysicsCamera* camEntity, f32 size, f32 mass, f32 velocity)
{
	return throwTestCube(camEntity->getCameraNode(), size, mass, velocity);
}

IPhysicsEntity* CPhysics::throwTestCube(CPhysicsCamera* camEntity)
{
	return throwTestCube(camEntity->getCameraNode(), 1.0f, 10.0f, 5.0f);
}

/// Drops a test sphere.
//! As above but allows you to specify the size and mass. The size in this case refers to the diameter, so a size
//! of 10.0f would create a sphere with diameter 10 (radius 5).
IPhysicsEntity* CPhysics::throwTestCube(ICameraSceneNode* cam, f32 size, f32 mass, f32 velocity)
{
	SPhysicsPrimitive* cube = new SPhysicsPrimitive;

	cube->primitiveType = EPT_CUBE;
	cube->bodyType = EBT_DYNAMIC;
	cube->mass = mass;
	cube->node = m_device->getSceneManager()->addCubeSceneNode(size);
	cube->node->setMaterialFlag(EMF_LIGHTING, false);
	cube->size = vector3df(size,size,size);
	cube->scale = vector3df(0.5f,0.5f,0.5f);

	IPhysicsEntity* entity = addEntity(cube);

	entity->setPosition(cam->getPosition());

	vector3df vel = (cam->getTarget() - cam->getPosition()).normalize() * velocity;
	NewtonBodySetVelocity(entity->getBody(), &vel.X);

	return entity;
}

/// Drops a test sphere.
//! This is for testing and debugging - a test sphere is created using the built-in Irrlicht primitive and dropped from the
//! specified position. The ISceneManager pointer should be the one from the IrrlichtDevice that you are using. The size of
//! the sphere (in this case the diameter) will be 10 and the mass will be 10.
IPhysicsEntity* CPhysics::throwTestSphere(CPhysicsCamera* camEntity, f32 size, f32 mass, f32 velocity)
{
	return throwTestSphere(camEntity->getCameraNode(), size, mass, velocity);
}

IPhysicsEntity* CPhysics::throwTestSphere(CPhysicsCamera* camEntity)
{
	return throwTestSphere(camEntity->getCameraNode(), 1.0f, 10.0f, 5.0f);
}

/// Drops a test sphere.
//! As above but allows you to specify the size and mass. The size in this case refers to the diameter, so a size
//! of 10.0f would create a sphere with diameter 10 (radius 5).
IPhysicsEntity* CPhysics::throwTestSphere(ICameraSceneNode* cam, f32 size, f32 mass, f32 velocity)
{
	SPhysicsPrimitive* sphere = new SPhysicsPrimitive;

	sphere->primitiveType = EPT_SPHERE;
	sphere->bodyType = EBT_DYNAMIC;
	sphere->mass = mass;
	sphere->node = m_device->getSceneManager()->addSphereSceneNode(size);
	sphere->node->setMaterialFlag(EMF_LIGHTING, false);
	sphere->size = vector3df(size,size,size);
	sphere->scale = vector3df(0.5f,0.5f,0.5f);

	IPhysicsEntity* entity = addEntity(sphere);

	entity->setPosition(cam->getPosition());

	vector3df vel = (cam->getTarget() - cam->getPosition()).normalize() * velocity;
	NewtonBodySetVelocity(entity->getBody(), &vel.X);

	return entity;
}

CMaterialManager* CPhysics::getMaterialManager()
{
	return m_materialManager;
}

void CPhysics::debug(bool doDebug)
{
	m_showDebug = doDebug;
}

void CPhysics::NewtonDebugCollision (const NewtonBody* body, int vertexCount, const float* faceVertec, int id)
{
	int i;
	vector3df p0;
	vector3df p1;

	i = vertexCount - 1;
	p0.X = faceVertec[i * 3];
	p0.Y = faceVertec[i * 3 + 1];
	p0.Z = faceVertec[i * 3 + 2];

	for (i = 0; i < vertexCount; i++)
	{
		p1.X = faceVertec[i * 3];
		p1.Y = faceVertec[i * 3 + 1];
		p1.Z = faceVertec[i * 3 + 2];

		m_device->getVideoDriver()->draw3DLine (p0 * NewtonToIrr, p1 * NewtonToIrr, SColor(255, 255, 255, 0));
 		p0 = p1;
	}
}

void CPhysics::NewtonDebugBody (const NewtonBody* body)
{
	matrix4 mat;
	SMaterial material;

	material.TextureLayer[0].Texture = 0; // Irrlicht 1.3
	material.Lighting = false;

	m_device->getVideoDriver()->setTransform(ETS_WORLD, mat);
	m_device->getVideoDriver()->setMaterial(material);

	NewtonBodyForEachPolygonDo (body, NewtonDebugCollision);
}
