#ifndef __shared_physcommon_h
#define __shared_physcommon_h
#include <chrono>
#include <list>
#include <assert.h>
#include <btBulletDynamicsCommon.h>
extern "C" {
  #include <lua.h>
  #include <lauxlib.h>
  #include <lualib.h>
}
#include <shared/lua_api/common.hpp>
#include "physcommon.hpp"

using namespace std::chrono;

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

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(-100000,-100000,-100000),btVector3(100000,100000,100000));
	//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);
	broadphase ->getOverlappingPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());
	//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");
	assert(lua_gettop(L) == 0);
	if(f){
		//printf("Updating the position of %llu objects\n", Objects.size());
		// Relay the object's orientation to irrlicht
		for(std::list<btCollisionObject *>::iterator it = Objects.begin(); it != Objects.end(); ++it) {
			(*f)(*it);
		}
	}
	assert(lua_gettop(L) == 0);

	//Call phy.oncollide(obj1, obj2, point1, point2, normal2)
	lua_getglobal(L,"phys");//phys
	lua_getfield(L,-1,"colliders");//phys,colliders
	int nummanifolds = World->getDispatcher()->getNumManifolds();
	pusherrorfunc(L);//{phys},{colliders},errfunc()
	for(int i = 0; i < nummanifolds; i++){
		//printf("Looking at manifold %d, top is %d\n", i, lua_gettop(L));

		lua_getfield(L,-3,"oncollide");//{phys},{colliders},errfunc(),oncollide()
		if(lua_isnil(L,-1)){
			lua_pop(L,4);
			return;
		}
		btPersistentManifold *mf = World->getDispatcher()->getManifoldByIndexInternal(i);
		btCollisionObject *oa = (btCollisionObject*)mf->getBody0();
		btCollisionObject *ob = (btCollisionObject*)mf->getBody1();
		btManifoldPoint mp = mf->getContactPoint(i);
		btVector3 pa = mp.getPositionWorldOnA();
		btVector3 pb = mp.getPositionWorldOnB();
		btVector3 pn = mp.m_normalWorldOnB;
		lua_pushlightuserdata(L,oa);//{phys},{colliders},errfunc(),oncollide(),ud_oa
		lua_gettable(L,-4);//{phys},{colliders},errfun(),concollide(),{oa}
		lua_pushlightuserdata(L,ob);//{phys},{colliders},errfunc(),oncollide(),{oa},ud_ob
		lua_gettable(L,-5);//{phys},{colliders},errfunc(),oncollide(),{oa},{ob}
		pushvector3d(L,pa.x(),pa.y(),pa.z());
		pushvector3d(L,pb.x(),pb.y(),pb.z());
		pushvector3d(L,pn.x(),pn.y(),pn.z());//{phys},{colliders},errfunc(),oncollide(),{oa},{ob},{pa},{pb},{normal}
		int err = lua_pcall(L,5,0,-7);//{phys},{colliders},errfunc()
		if(err)
			printf("Failed to call oncollide\n");
	}
	lua_pop(L,3);
	assert(lua_gettop(L) == 0);
}

high_resolution_clock::time_point t1 = high_resolution_clock::now();
void gameloop_phys(void(*f)(btCollisionObject *)){
	assert(lua_gettop(L) == 0);
	//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;
	assert(lua_gettop(L) == 0);
}
#endif