#include <stdio.h>
#include <stdlib.h>
#include <list>
extern "C" {
  #include <lua.h>
  #include <lauxlib.h>
  #include <lualib.h>
}
#include <btBulletDynamicsCommon.h>
#include "bghostobject.hpp"
#include <shared/lua_api/common.hpp>

extern btDiscreteDynamicsWorld* World;
extern std::list<btCollisionObject*> Objects;
//extern std::list<btGhostObject*> Ghosts;

/*
static LBPhysNode* checkisbphysbox(lua_State* L, int index){
  void* ud = luaL_checkudata(L,index,"phys.physbox");
  luaL_argcheck(L,ud != NULL, index, "'phys.physbox' expected");
  return (LBPhysNode*) ud;
}
*/

/*
static LISceneNode* checkismesh(lua_State* L){
  return checkismesh(L,1);
}
*/
// ud_btGhostObject :: ({v3 size}, {v3 origin})
void makeghostobject(lua_State* L){
	double px,py,pz; //position
	double sx,sy,sz; //size
	
	popvector3d(L,&px,&py,&pz);//{v3_size}
	//printf("Got position: (%f,%f,%f)\n",px,py,pz);
	popvector3d(L,&sx,&sy,&sz);//

	btVector3 vshape = btVector3(sx * 0.5f, sy * 0.5f, sz * 0.5f);
	//printf("Got size: (%f,%f,%f)\n",sx,sy,sz);
	btVector3 pos = btVector3(px,py,pz);

	// Set the initial position of the object
	btTransform transform = btTransform(btQuaternion(0,0,0,1),pos);
	//transform.setIdentity();
	//transform.setOrigin(pos);
    
	// Create the shape
	btCollisionShape* shape = new btBoxShape(vshape);
	if(!shape){
		//printf("no shape\n");
	}

	// Add mass
	btVector3 localinertia = btVector3(0,0,0);
	shape->calculateLocalInertia(1, localinertia);

	//cinfo.m_friction = 0;
	btGhostObject *ghost = new btGhostObject();
	ghost->setCollisionShape(shape);
	ghost->setWorldTransform(transform);
	ghost->setCollisionFlags(
			btCollisionObject::CollisionFlags::CF_NO_CONTACT_RESPONSE | 
			btCollisionObject::CollisionFlags::CF_KINEMATIC_OBJECT
			);
	World->addCollisionObject(ghost, btBroadphaseProxy::SensorTrigger, btBroadphaseProxy::AllFilter & ~btBroadphaseProxy::SensorTrigger);
    
	//printf("Added rigid body to world: %p\n",World);
	Objects.push_back(ghost);

	lua_pushlightuserdata(L,ghost);//ud_ghost
}

// phys.newghostobject(vector3 size, vector3 origin)
int newghostobject(lua_State* L){
	//printf("Createing bphysbox!\n");
	//Create it's lua representation
	makeghostobject(L);//ud_btGhostObject
	btGhostObject* ghost = (btGhostObject*)lua_touserdata(L,-1);
	lua_pop(L,1);
	lua_newtable(L);//{}
	lua_pushlightuserdata(L,ghost);//ud_btGhostObject
	lua_setfield(L,-2,"collider");//{}

	//Set it's metatable
	luaL_getmetatable(L, "phys.ghost");//{},{phys.ghost}
	lua_setmetatable(L, -2);//{}

  	return 1;
}

//{phys.physbox}:delete()
static int delbghostobject(lua_State* L){//self
	//printf("Attempting to delete physbox\n");
	lua_getfield(L,-1,"collider");//self,ud_rigidbody
	btGhostObject* r = (btGhostObject*)lua_touserdata(L,-1);//self,ud_rigidbody
	delete r->getCollisionShape();
	delete r;

	return 0;
}

// physbox:setpos({v3 pos})
static int bghostsetpos(lua_State *L){//self,{v3 pos}
	double nx,ny,nz;
	popvector3d(L,&nx,&ny,&nz);//self

	lua_getfield(L,-1,"collider");//self,ud_ghost
	btGhostObject *ghost = (btGhostObject*)lua_touserdata(L,-1);//self
	btTransform bt = ghost->getWorldTransform();

	btVector3 to = btVector3(nx,ny,nz);
	bt.setOrigin(to);
	ghost->setWorldTransform(bt);
	ghost->activate();

	lua_pop(L,1);//
	return 0;
}

// {v3 pos} :: physbox:getpos()
static int bghostgetpos(lua_State *L){//self
	//printf("Physics box set pos called\n");
	lua_getfield(L,-1,"collider");//self,ud_ghost
	btGhostObject* i = (btGhostObject*)lua_touserdata(L,-1);//self,ud_ghost
	btTransform bt = i->getWorldTransform();
	btVector3 bv = bt.getOrigin();
	lua_pop(L,2);//
	pushvector3d(L,bv.x(),bv.y(),bv.z());//{}

	return 1;
}

//ghost:getoverlapping()
int bghostoverlapping(lua_State *L){
	lua_getfield(L,-1,"collider");//{ghost}
	btGhostObject *ghost = (btGhostObject*)lua_touserdata(L,-1);//{ghost},ud_ghost
	lua_pop(L,2);//
	lua_newtable(L);//{}
	btAlignedObjectArray<btCollisionObject *> ob = ghost->getOverlappingPairs();
	printf("Getting %d overlapping object\n",ob.size());
	for(int i = 0; i < ob.size(); i++){
		printf("Looking at object %d\n",i);
		btCollisionObject *co = ob[i];
		lua_getglobal(L,"phys");//{},{phys}
		lua_getfield(L,-1,"colliders");//{},{phys},{phys.colliders}
		lua_pushnumber(L,i+1);//}{},{phys},{phys.colliders},i
		lua_pushlightuserdata(L,co);//{},{phys},{phys.colliders},i,ud_co
		lua_gettable(L,-3);//{},{phys},{phys.colliders},i,{collider=ud_co}
		if(lua_isnil(L,-1)){
			printf("Failed to find collider, failing...\n");
			lua_pushstring(L,"Failed to find collider we are overlapping");
			lua_error(L);
		}
		lua_settable(L,-5);//{i={collider=co}},{phys},{phys.colliders}
		lua_pop(L,2);//{i={...}}
	}
	printf("Finished adding %d overlapping objects to array...\n",(int)lua_objlen(L,-1));
	return 1;
}

static const luaL_reg bghost_m[] = {
  {"getpos",               bghostgetpos},
  {"setpos",               bghostsetpos},
  {"getoverlapping",       bghostoverlapping},
  {"delete", 		   delbghostobject},
  {0, 0},
};

void bghostobject_register(lua_State* L){//
	//printf("Registered bphysbox\n");

	luaL_newmetatable(L, "phys.ghost");//{phys.physbox}
	lua_newtable(L);//{phys.physbox},{}
	luaL_register(L,NULL,bghost_m);//{phys.physbox},{}
	lua_setfield(L,-2,"__index");//{phys.physbox}

	lua_pop(L,1);//

	lua_getglobal(L,"phys");//{}
	lua_pushcfunction(L,newghostobject);//{},newghostobject()
	lua_setfield(L,-2,"newghostbox");//{}

	lua_pop(L,1);
}