#include "World.h"
#include "PhysicalObject.h"
#include "../Utility/Timer.h"

#include <btBulletDynamicsCommon.h>

#include <vector>
#include <algorithm>

//////////////////////////////////////////////////////////////
namespace Dubious {
namespace Physics {


World::World()
{
	m_pCollisionConfig	= new btDefaultCollisionConfiguration();
	m_pDispatcher		= new btCollisionDispatcher(m_pCollisionConfig);
	m_pPairCache		= new btDbvtBroadphase();
	m_pSolver			= new btSequentialImpulseConstraintSolver;
	m_pWorld			= new btDiscreteDynamicsWorld( m_pDispatcher, m_pPairCache, 
													   m_pSolver, m_pCollisionConfig);
}

World::~World()
{
	while( m_Objects.size() > 0 ){
		RemoveObject( m_Objects.front() );
	}
	delete m_pWorld;
	delete m_pSolver;
	delete m_pPairCache;
	delete m_pDispatcher;
	delete m_pCollisionConfig;
}

void World::StepSimulation( float Elapsed, unsigned int &Bullet, unsigned int &Maint, unsigned int &Handling )
{
	Utility::Timer DebugTimer;

	DebugTimer.Start();
	m_pWorld->stepSimulation( Elapsed, 7 );
	Bullet += DebugTimer.Elapsed();

	if (!m_OnCollision) {
		return;
	}

	DebugTimer.Start();
	// Go through the world and collect all of the collision pairs
	// before calling any events.  This is so if someone wants to remove
	// them during the callback, it won't affect the internal state.
	std::vector<std::pair<PhysicalObjectPtr,PhysicalObjectPtr> > CollisionPairs;
	int NumManifolds = m_pWorld->getDispatcher()->getNumManifolds();
	for( int i=0; i<NumManifolds; ++i ){
		btPersistentManifold* ContactManifold = m_pWorld->getDispatcher()->getManifoldByIndexInternal(i);
		const btCollisionObject* obA = ContactManifold->getBody0();
		const btCollisionObject* obB = ContactManifold->getBody1();

		// inefficient, but fine for now
		PhysicalObjectPtr pA, pB;
		for( auto iter = m_Objects.begin(), end = m_Objects.end(); iter != end; ++iter ){
			 if ((*iter)->m_pBody == obA) {
				 pA = *iter;
			 } 
			 else if ((*iter)->m_pBody == obB) {
				 pB = *iter;
			 }
		}
		CollisionPairs.push_back( std::make_pair( pA, pB ) );
	}
	Maint += DebugTimer.Elapsed();

	DebugTimer.Start();
	// signal the events
	for( size_t i=0; i<CollisionPairs.size(); ++i ){
		m_OnCollision( CollisionPairs[i].first, CollisionPairs[i].second );
	}
	Handling += DebugTimer.Elapsed();
}

void World::AddObject( PhysicalObjectPtr pObject )
{
	m_Objects.push_back( pObject );
	m_pWorld->addRigidBody( pObject->m_pBody );
}

void World::RemoveObject( PhysicalObjectPtr pObject )
{
	m_pWorld->removeCollisionObject( pObject->m_pBody );
	m_Objects.remove( pObject );
}

void World::RemoveAllObjects()
{
	std::for_each( m_Objects.begin(), m_Objects.end(), [this]( PhysicalObjectPtr pObject ) {
		m_pWorld->removeCollisionObject( pObject->m_pBody );
	});
	m_Objects.clear();
}

void World::SetGravity( const Math::Vector& Gravity )
{
	m_pWorld->setGravity( Gravity.Vector() );
}

}
}
