#include <chrono>
#include <list>
#include <btBulletDynamicsCommon.h>

#include "physcommon.hpp"

using namespace std::chrono;

btDiscreteDynamicsWorld* World;
std::list<btRigidBody*> Objects;

// 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();
}

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

void phys_genesis(){
	BroadPhase = new btAxisSweep3(btVector3(-1000,-1000,-1000),btVector3(1000,1000,1000));
	//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(void(*f)(btRigidBody*)){
	ClearObjects(World,Objects,f);
	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)(btRigidBody*)) {
	World->stepSimulation(TDeltaTime * 0.2f, 60);
	if(f){
		// Relay the object's orientation to irrlicht
		for(std::list<btRigidBody *>::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)(btRigidBody*)){
	//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;
}