#include "RigidBodySimulation.h"

// local includes
//#include "scriptsystem.h"

// library includes
#include "Utilities/StringUtilities.h"
#include "Utilities/Log.h"
//#include "Utilities/xmlconfig.h"

// system includes
#include <iostream>

using namespace std;

namespace BlueCore
{
static RigidBodySimulation* gRigidBodySimulation;

//------------------------------------------------------------------------------
RigidBody::RigidBody(RigidBodySimulation* simulation, dWorldID world,
		dSpaceID space) :
	_Body( 0), _Space( 0), _Simulation(simulation)
{
	_Body = dBodyCreate(world);
	_Space = dHashSpaceCreate(space);
	dHashSpaceSetLevels(_Space, -10, 10);
	dBodySetData(_Body, this);
	clog << ">>> RigidBody constructed..."<< endlog;
}

//------------------------------------------------------------------------------
RigidBody::~RigidBody()
{
	dSpaceDestroy(_Space);
	dBodyDestroy(_Body);

	std::vector<CollisionMesh>::iterator iter;
	for (iter = _CollisionMeshes.begin(); iter != _CollisionMeshes.end(); iter++)
	{
		dGeomTriMeshDataDestroy((*iter).trimeshdata);
	}

	clog << ">>> RigidBody destructed..."<< endlog;
}

//------------------------------------------------------------------------------

const Vector3 &RigidBody::getPosition() const
{
	return _Position;
}

//------------------------------------------------------------------------------
void RigidBody::setPosition(const Vector3 &v)
{
	dBodySetPosition(_Body, v.x, v.y, v.z);
	_Position = v;
}

//------------------------------------------------------------------------------
const Vector3 &RigidBody::getLinearVelocity() const
{
	return _LinearVelocity;
}

//------------------------------------------------------------------------------
void RigidBody::setLinearVelocity(const Vector3 &v)
{
	dBodySetLinearVel(_Body, v.x, v.y, v.z);
	_LinearVelocity = v;
}

//------------------------------------------------------------------------------
const Quaternion &RigidBody::getOrientation() const
{
	return _Orientation;
}

//------------------------------------------------------------------------------
void RigidBody::setOrientation(const Quaternion &q)
{

	dBodySetQuaternion(_Body, ( const dQuaternion & ) q );

	_Orientation = q;
}

//------------------------------------------------------------------------------
const Vector3 &RigidBody::getAngularVelocity() const
{
	return _AngularVelocity;
}

//------------------------------------------------------------------------------
void RigidBody::applyGlobalForce(const Vector3 &force, const Vector3 &point)
{
	dBodyAddForceAtPos(_Body, force.x, force.y, force.z, point.x, point.y,
			point.z);
}

//------------------------------------------------------------------------------
void RigidBody::applyGlobalForce(const Vector3 &force)
{
	dBodyAddForce(_Body, force.x, force.y, force.z);
}

//------------------------------------------------------------------------------
void RigidBody::applyLocalForce(const Vector3 &force, const Vector3 &point)
{
	if (point.isZero() )
	{
		dBodyAddRelForce(_Body, force.x, force.y, force.z);
	}
	else
	{
		dBodyAddRelForceAtRelPos(_Body, force.x, force.y, force.z, point.x,
				point.y, point.z);
	}
}

//------------------------------------------------------------------------------
void RigidBody::applyLocalForce(const Vector3 &force)
{
	dBodyAddRelForce(_Body, force.x, force.y, force.z);
}

//------------------------------------------------------------------------------
Scalar RigidBody::getMass() const
{
	dMass m;
	dBodyGetMass(_Body, &m);
	return m.mass;
}

//------------------------------------------------------------------------------
void RigidBody::saveState()
{
	// save position

	const Scalar *p = dBodyGetPosition(_Body);
	_Position.x = p[0];
	_Position.y = p[1];
	_Position.z = p[2];

	// save linear velocity
	const Scalar *v = dBodyGetLinearVel(_Body);
	_LinearVelocity.x = v[0];
	_LinearVelocity.y = v[1];
	_LinearVelocity.z = v[2];

	// save orientation
	const Scalar *q = dBodyGetQuaternion(_Body);
	_Orientation.w = q[0];
	_Orientation.x = q[1];
	_Orientation.y = q[2];
	_Orientation.z = q[3];

	// save angular velocity
	const Scalar *a = dBodyGetAngularVel(_Body);
	_AngularVelocity.x = a[0];
	_AngularVelocity.y = a[1];
	_AngularVelocity.z = a[2];
}

//------------------------------------------------------------------------------
unsigned int RigidBody::addCollisionMesh(const std::string &meshname,
		Scalar density)
{
	if (_Simulation.valid() == false)
		return 0;

	const RigidBodySimulation::Trimesh *trimesh =
			_Simulation->getTrimesh(meshname);

	if (trimesh)
	{
		CollisionMesh collisionmesh;

		collisionmesh.trimeshdata = dGeomTriMeshDataCreate();
		dGeomTriMeshDataBuildSingle(collisionmesh.trimeshdata,
				trimesh->vertices.const_data(), 3 * sizeof(float),
				trimesh->vertices.count() / 3, trimesh->indices.const_data(),
				trimesh->indices.count(), 3 * sizeof(int));

		collisionmesh.geom = dCreateTriMesh(_Space, collisionmesh.trimeshdata,
				0, 0, 0);
		dGeomSetBody(collisionmesh.geom, _Body);

		dMass mass;
		dMassSetTrimesh( &mass, density, collisionmesh.geom);
		//dMassSetBox( &mass, density, 5.0, 5.0, 5.0 );
		dBodySetMass(_Body, &mass);

		_CollisionMeshes.push_back(collisionmesh);
		return _CollisionMeshes.size();
	}

	return 0;
}

//------------------------------------------------------------------------------
void RigidBody::setCollisionMeshPosition(unsigned int geom,
		const Vector3 &position)
{
	if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
	{
		dGeomSetOffsetPosition(_CollisionMeshes[geom-1].geom, position.x,
				position.y, position.z);
	}
}

//------------------------------------------------------------------------------
void RigidBody::setCollisionMeshRotation(unsigned int geom,
		const Quaternion &rotation)
{
	if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
	{
		dGeomSetOffsetQuaternion(_CollisionMeshes[geom-1].geom,
				( dReal * ) &rotation );
	}
}

//------------------------------------------------------------------------------
void RigidBody::getCollisionMeshPosition(unsigned int geom, Vector3 &position)
{
	if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
	{
		const Scalar*p = dGeomGetOffsetPosition(_CollisionMeshes[geom-1].geom);
		position.x = p[0];
		position.y = p[1];
		position.z = p[2];
	}
}

//------------------------------------------------------------------------------
void RigidBody::getCollisionMeshRotation(unsigned int geom,
		Quaternion &rotation)
{
	if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
	{
		dGeomGetOffsetQuaternion(_CollisionMeshes[geom-1].geom,
				( dReal * ) &rotation );
	}
}

//------------------------------------------------------------------------------
void RigidBody::enableCollisionMesh(unsigned int geom)
{
	if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
	{
		dGeomEnable(_CollisionMeshes[geom-1].geom);
	}
}

//------------------------------------------------------------------------------
void RigidBody::disableCollisionMesh(unsigned int geom)
{
	if ( (geom > 0) && (geom <= _CollisionMeshes.size() ))
	{
		dGeomDisable(_CollisionMeshes[geom-1].geom);
	}
}

//------------------------------------------------------------------------------
unsigned int RigidBody::getCollisionMeshId(dGeomID geom)
{
	for (unsigned int i = 0; i < _CollisionMeshes.size(); i++)
	{
		if (_CollisionMeshes[i].geom == geom)
			return i + 1;
	}

	return 0;
}

//------------------------------------------------------------------------------
HSQOBJECT RigidBody::getCollisionHandler()
{
	return _CollisionHandler;
}

//------------------------------------------------------------------------------
void RigidBody::setCollisionHandler(HSQOBJECT handler)
{
	_CollisionHandler = handler;
}

//------------------------------------------------------------------------------
RigidBodySimulation::RigidBodySimulation(ScriptSystem *scriptsystem) :
	_world( 0), _space( 0), _contactgroup( 0), _delta( 0.0), _steps( 0),
			_ScriptSystem(scriptsystem)
{
	dInitODE();

	gRigidBodySimulation = this;
	//XmlConfig config("config.xml");

	clog << ">>> initialize RigidBodySimulation..."<< endline;

	Scalar erp = 1.0, cfm = 0.0;
	_stepSize = 0.02;
	_maxContacts = 5;
	/*
	 config.getDouble("RigidBodySimulation", "ERP", erp, 1.0);
	 config.getDouble("RigidBodySimulation", "CFM", cfm, 0.0);
	 config.getDouble("RigidBodySimulation", "StepSize", _stepSize, 0.02);
	 config.getInt("RigidBodySimulation", "MaxContacts", _maxContacts, 5);
	 */
	// sanity check

	if (erp > 1.0)
		erp = 1.0;
	else if (erp < 0.0001)
		erp = 0.0001;

	clog << "    ERP: "<< erp << endline;

	// sanity check
	if (cfm > 1.0)
		cfm = 1.0;
	else if (cfm < 0.0)
		cfm = 0.0;

	clog << "    CFM: "<< cfm << endline;

	// sanity check
	if (_stepSize < 0.0001)
		_stepSize = 0.0001;
	else if (_stepSize > 1.0)
		_stepSize = 1.0;

	clog << "    step size: "<< _stepSize << endline;

	// sanity check
	if (_maxContacts < 1)
		_maxContacts = 1;
	else if (_maxContacts > 500)
		_maxContacts = 500;

	clog << "    max counts: "<< _maxContacts << endline;

	_world = dWorldCreate();

	dWorldSetERP(_world, erp);

	dWorldSetCFM(_world, cfm);

	_space = dHashSpaceCreate( 0);

	dHashSpaceSetLevels(_space, -10, 10);

	_contactgroup = dJointGroupCreate( 0);
	/*
	 // Bullet
	 btVector3 worldAabbMin(-100000,-100000,-100000);
	 btVector3 worldAabbMax(100000,100000,100000);
	 btOverlappingPairCache* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,32766);
	 btCollisionDispatcher* dispatcher = new	btCollisionDispatcher();
	 btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
	 _World = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver);
	 */
}

//------------------------------------------------------------------------------
RigidBodySimulation::~RigidBodySimulation()
{

	if (_contactgroup)
		dJointGroupDestroy(_contactgroup);

	if (_space)
		dSpaceDestroy(_space);

	if (_world)
		dWorldDestroy(_world);

	std::map<std::string, Trimesh *>::iterator iter;
	for (iter = _Trimeshes.begin(); iter != _Trimeshes.end(); iter++)
	{
		Trimesh *trimesh = iter->second;
		if (trimesh)
			delete trimesh;
	}

	_Trimeshes.clear();

	dCloseODE();

	gRigidBodySimulation = 0;

	clog << ">>> RigidBodySimulation destructed ..."<< endline;
}

//------------------------------------------------------------------------------
RigidBody *RigidBodySimulation::createRigidBody()
{
	RigidBody *rigidBody = new RigidBody ( this, _world, _space );

	_bodies.push_back(rigidBody);

	return rigidBody;
}

//------------------------------------------------------------------------------
void RigidBodySimulation::deleteRigidBody(RigidBody *rigidBody)
{
#ifdef DEBUG_PHYSICS
	clog << ">>> delete rigid body" << endline;
#endif
	_bodies.remove(rigidBody);
	delete rigidBody;
}

//------------------------------------------------------------------------------
void RigidBodySimulation::removeAll()
{
	std::list<RigidBody *>::iterator i;

	for (i = _bodies.begin(); i != _bodies.end(); i++)
		delete *i;

	_bodies.clear();
}

//------------------------------------------------------------------------------
void RigidBodySimulation::saveStates()
{
	std::list<RigidBody *>::iterator i;

	for (i = _bodies.begin(); i != _bodies.end(); i++)
		( *i )->saveState();
}

//------------------------------------------------------------------------------
void RigidBodySimulation::nearCallback(void *data, dGeomID o1, dGeomID o2)
{
	if (gRigidBodySimulation == 0)
		return;

	if (dGeomIsSpace(o1) && dGeomIsSpace(o2) )
	{

		// colliding a space with something
		dSpaceCollide2(o1, o2, data, &nearCallback);
		/*
		 // collide all geoms internal to the space(s)
		 if( dGeomIsSpace(o1) )
		 dSpaceCollide( (dxSpace *)o1, data, &nearCallback );
		 
		 if( dGeomIsSpace(o2) )
		 dSpaceCollide( (dxSpace *)o2, data,&nearCallback );
		 */
	}
	else
	{
		static Buffer<dContact> contacts;

		if (contacts.count() == 0)
			contacts.create(gRigidBodySimulation->getMaxContacts());

		dBodyID b1, b2;
		b1 = dGeomGetBody(o1);
		b2 = dGeomGetBody(o2);

		// exit without doing anything if the two bodies are connected by a joint
		if (b1 && b2 && dAreConnected(b1, b2) )
			return;

		for (unsigned int i = 0; i < contacts.count(); i++)
		{
			contacts[i].surface.mode = 0; //dContactBounce;
			contacts[i].surface.mu = 1.0;
			contacts[i].surface.mu2 = 0;
			contacts[i].surface.bounce = 0.1;
			contacts[i].surface.bounce_vel = 0.1;
			contacts[i].surface.soft_cfm = 0.01;
		}

		unsigned int collisions = dCollide(o1, o2, contacts.count(),
				&contacts[0].geom, sizeof(dContact));

		//clog << "collide" << endline;

		unsigned int actual_collisions = 0;

		for (unsigned int i = 0; i < collisions; i++)
		{
			clog << "depth: "<< contacts[i].geom.depth<< endline;
			dJointID joint = dJointCreateContact(gRigidBodySimulation->_world,
					gRigidBodySimulation->_contactgroup, &contacts[i]);
			dJointAttach(joint, b1, b2);
		}

#ifdef DEBUG_PHYSICS
		if ( collisions == contacts.count() )
		clog << "!!! Max collisions reached!" << endline;

#endif

		if (collisions > 0)
		{
			//clog << "rbsim handle collision" << endline;
			//ScriptManager::getSingleton()->handleCollision ( o1, o2 );
		}
	}
}

//------------------------------------------------------------------------------
int RigidBodySimulation::getSteps()
{
	return _steps;
}

//------------------------------------------------------------------------------
Scalar RigidBodySimulation::getStepSize()
{
	return _stepSize;
}

//------------------------------------------------------------------------------
int RigidBodySimulation::getMaxContacts()
{
	return _maxContacts;
}

//------------------------------------------------------------------------------
void RigidBodySimulation::updateSteps(Scalar delta)
{
	_delta += delta;

	if (_delta < 0.0)
		return;

	_steps = ( int ) floor(_delta / _stepSize);

	if (_steps > 10)
		_steps = 10;

	_delta -= _steps * _stepSize;
}

//------------------------------------------------------------------------------
bool RigidBodySimulation::step()
{
	if (_steps > 0)
	{
		dSpaceCollide(_space, 0, &nearCallback);
		dWorldQuickStep(_world, _stepSize);
		StepSignal(_stepSize);
		dJointGroupEmpty(_contactgroup);
		_steps--;
	}

	if (_steps > 0)
		return true;

	return false;
}

//------------------------------------------------------------------------------
const RigidBodySimulation::Trimesh *RigidBodySimulation::getTrimesh(
		const std::string &name)
{
	Trimesh *trimesh = 0;

	std::map<std::string, Trimesh *>::const_iterator result;
	result = _Trimeshes.find(name);

	if (result != _Trimeshes.end() )
	{
		trimesh = result->second;
	}
	/*
	 if ( trimesh == 0 )
	 {
	 MeshLoader loader;

	 clog << ">>> loading collision trimesh: " << name << endline;
	 loader.load ( name + ".3ds" );

	 if ( loader.objects().count() > 0 )
	 {
	 trimesh = new Trimesh();

	 MeshLoader::Object &object = loader.objects().item ( 0 );

	 clog << "    " << object.vertices.count() << " Vertices" << endline;
	 trimesh->vertices.create ( object.vertices.count() * 3 );

	 clog << "    " << object.faces.count() * 3 << " Indices" << endline;
	 trimesh->indices.create ( object.faces.count() * 3 );

	 unsigned int i;

	 for ( i = 0; i < object.vertices.count(); i++ )
	 {
	 trimesh->vertices[i*3] = -object.vertices[i].x;
	 trimesh->vertices[i*3+1] = object.vertices[i].y;
	 trimesh->vertices[i*3+2] = object.vertices[i].z;
	 }

	 for ( i = 0; i < object.faces.count(); i++ )
	 {
	 trimesh->indices[i*3] = object.faces[i].a;
	 trimesh->indices[i*3+2] = object.faces[i].b;
	 trimesh->indices[i*3+1] = object.faces[i].c;
	 }

	 _Trimeshes[name] = trimesh;
	 }
	 }
	 */
	return trimesh;
}

} // namespace BlueCore