#ifndef __shared_physcommon_h
#define __shared_physcommon_h
#include <chrono>
#include <list>
#include <btBulletDynamicsCommon.h>

#include "physcommon.hpp"

using namespace std::chrono;

btDiscreteDynamicsWorld* World;
std::list<btCollisionObject*> Objects;
std::list<btKinematicCharacterController*> Chars;

extern void dropCollisionObject(btCollisionObject* b);

// Removes all objects from the world
//void ClearObjects(btDiscreteDynamicsWorld* wr, std::list<btRigidBody*> objs, void(*f)(btRigidBody*)) {

	//for(std::list<btRigidBody *>::iterator Iterator = objs.begin(); Iterator != objs.end(); ++Iterator) {
		//btRigidBody *Object = *Iterator;

		//if(f){
			//(*f)(Object);
		//}

		//// Remove the object from the world
		//wr->removeRigidBody(Object);
		//delete Object;
	//}
	//objs.clear();
//}
void ClearObjects(){
	for(std::list<btCollisionObject*>::iterator itr = Objects.begin(); itr != Objects.end(); ++itr){
		dropCollisionObject(*itr);
		World->removeCollisionObject(*itr);
		delete *itr;
	}
	Objects.clear();
}

btBroadphaseInterface* broadphase;
btDefaultCollisionConfiguration* CollisionConfiguration;
btCollisionDispatcher* Dispatcher;
btSequentialImpulseConstraintSolver* Solver;

void phys_genesis(){
	broadphase = new btAxisSweep3(btVector3(-1000,-1000,-1000),btVector3(1000,1000,1000));
	broadphase ->getOverlappingPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());
	//printf("Broadphase\n");
	CollisionConfiguration = new btDefaultCollisionConfiguration();
	//printf("Collision config\n");
	Dispatcher = new btCollisionDispatcher(CollisionConfiguration);
	//printf("Dispatcher\n");
	Solver = new btSequentialImpulseConstraintSolver();
	//printf("Solver\n");
	World = new btDiscreteDynamicsWorld(Dispatcher, broadphase, Solver, CollisionConfiguration);
	//printf("Physics world init ok.\n");
	World->setGravity(btVector3(0,-10,0));
	//printf("Created physics world: %p\n",World);
}

void phys_shutdown(){
	ClearObjects();
	printf("cleared objects\n");
	delete broadphase;
	printf("deleted broadphase\n");
	delete CollisionConfiguration;
	printf("deleted collision config\n");
	delete Dispatcher;
	printf("Deleted dispatcher\n");
	delete Solver;
	printf("deleted solver\n");

	delete World; //Muah ha ha
	printf("deleted world\n");
  
}


// Runs the physics simulation.
// - TDeltaTime tells the simulation how much time has passed since the last frame so the simulation can run independently of the frame rate. Optionally pass in an argument that will be called on every rigidbody in the world
void UpdatePhysics(double TDeltaTime, void(*f)(btCollisionObject *)) {
	//printf("Pre step simulation\n");
	World->stepSimulation(TDeltaTime * 0.2f, 60);
	//printf("Done step simulation\n");
	if(f){
		// Relay the object's orientation to irrlicht
		for(std::list<btCollisionObject *>::iterator it = Objects.begin(); it != Objects.end(); ++it) {
			(*f)(*it);
		}
	}
}

high_resolution_clock::time_point t1 = high_resolution_clock::now();
void gameloop_phys(void(*f)(btCollisionObject *)){
	//printf("Doing phys gameloop\n");
	high_resolution_clock::time_point now = high_resolution_clock::now();
	duration<double> delta = now-t1;
	double steps = delta.count() * 10;
	UpdatePhysics(steps,f);
	t1 = now;
}
#endif