#include <stdio.h>
#include <stdlib.h>
#include <list>
extern "C" {
  #include <lua.h>
  #include <lauxlib.h>
  #include <lualib.h>
}
#include <btBulletDynamicsCommon.h>
#include <BulletDynamics/Character/btKinematicCharacterController.h>
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
#include "bcharactercontroller.hpp"
#include <shared/lua_api/common.hpp>

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

//{character} :: btKinematicCharacterController*
btKinematicCharacterController *popCharacter(lua_State *L){
	lua_getfield(L,-1,"type");//{char},"type"
	if(lua_isnil(L,-1)){
		lua_pushstring(L,"Tried to call a character method on something that had not 'type'");
		lua_error(L);
	}
	const char *s = lua_tostring(L,-1);//{char},"type"
	if(strcmp(s,"character")!= 0){
		printf("Tried to pop character when it was not a character!\n");
		lua_pushstring(L,"Tried to call a character method on a ");
		lua_pushstring(L,s);
		lua_concat(L,2);
		lua_error(L);
	}
	lua_getfield(L,-2,"character");//{char},"type",ud_character
	if(lua_isnil(L,-1)){
		printf("Failed to get a \"character\" field\n");
		lua_pushstring(L,"Character object was not set up correctly\n");
		lua_error(L);
	}
	btKinematicCharacterController *c = (btKinematicCharacterController*)lua_touserdata(L,-1);
	lua_pop(L,3);
	return c;
	
}
/*
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_character :: ({v3 size}, {v3 origin})
void makenewbcharactercontroller(lua_State* L){
	printf("Creating new character controller\n");
	//lua_pushstring(L,"Character controller is totally fucking broken for now\n");
	//lua_error(L);
	double px,py,pz; //position
	double sx,sy,sz; //size
	//double mass;
	
	//mass = lua_tonumber(L,-1);//{v3_size},{v3_origin},mass
	//lua_pop(L,1);//{v3_size},{v3_origin}
	//printf("Got mass: %f\n",mass);
	
	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);
	btTransform transform = btTransform(btQuaternion(0,0,0,1),pos);

	// Create the shape
	btConvexShape* cshape = new btBoxShape(vshape);



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

	// Create the rigid body object
	//btRigidBody::btRigidBodyConstructionInfo cinfo = btRigidBody::btRigidBodyConstructionInfo(
		//mass,
		//motionstate,
		//shape,
		//localinertia
	//);
	btPairCachingGhostObject *ghost = new btPairCachingGhostObject();
	ghost->setWorldTransform(transform);
	ghost->setCollisionShape(cshape);
	World->addCollisionObject(ghost,btBroadphaseProxy::CharacterFilter, btBroadphaseProxy::StaticFilter|btBroadphaseProxy::DefaultFilter);
	//ghost->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
	printf("Character controller created\n");
	btKinematicCharacterController *cc = new btKinematicCharacterController(ghost, cshape, 1, btVector3(0,1,0));
	//cc->setMaxSlope(3.14 / 4.0);
	//cinfo.m_friction = 0;
    
	// Add it to the world
	printf("About to add action\n");
	World->addAction(cc);
	printf("Finished adding action\n");
	//printf("Added rigid body to world: %p\n",World);
	printf("Added to Chars\n");
	//Chars.push_back(cc);
	//Objects.push_back(ghost);

	lua_pushlightuserdata(L,cc);//ud_cc
}

// char:getvelocity()
int bcharactergetvelocity(lua_State *L){
	btKinematicCharacterController *r = popCharacter(L);
	btVector3 v = r->getLinearVelocity();
	pushvector3d(L,v.x(),v.y(),v.z());
	return 1;
}

// char:setvelocity(v3 vel)
int bcharactersetvelocity(lua_State *L){
	double x,y,z;
	popvector3d(L,&x,&y,&z);
	btKinematicCharacterController *r = popCharacter(L);
	r->setLinearVelocity(btVector3(x,y,z));
	return 0;
}

// phys.newphysbox(vector3 size, vector3 origin, double mass)
int newbcharactercontroller(lua_State* L){
	//printf("Createing bphysbox!\n");
	//Create it's lua representation
	makenewbcharactercontroller(L);//ud_cc
	btKinematicCharacterController* r = (btKinematicCharacterController*)lua_touserdata(L,-1);//ud_cc
	lua_pop(L,1);//
	lua_newtable(L);//{}
	lua_pushlightuserdata(L,r);//{},ud_cc
	lua_setfield(L,-2,"character");//{character=ud_cc}

	lua_pushstring(L,"character");
	lua_setfield(L,-2,"type");//{character=ud_cc,type="character"}

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

  	return 1;
}

// char:setgravity(v3 gravity)
int bcharactersetgravity(lua_State *L){
	double x,y,z;
	popvector3d(L,&x,&y,&z);
	btKinematicCharacterController *c = popCharacter(L);
	c->setGravity(btVector3(x,y,z));
	return 0;
}

// char:getpos() :: v3
int bcharactergetpos(lua_State *L){
	btKinematicCharacterController *c = popCharacter(L);
	btVector3 pos = c->getGhostObject()->getWorldTransform().getOrigin();
	pushvector3d(L,pos.x(),pos.y(),pos.z());
	return 1;
}

// char:setpos(v3 pos)
int bcharactersetpos(lua_State *L){
	double x,y,z;
	popvector3d(L,&x,&y,&z);
	btKinematicCharacterController *c = popCharacter(L);
	c->warp(btVector3(x,y,z));
	//btTransform t = c->getGhostObject()->getWorldTransform();
	//t.setOrigin(btVector3(x,y,z));
	//c->getGhostObject()->setWorldTransform(t);
	return 0;
}

// char:onground()
int bcharacteronground(lua_State *L){
	btKinematicCharacterController *c = popCharacter(L);
	lua_pushboolean(L,c->onGround() == true ? 1 : 0);
	return 1;
}

// char:jump(v3 jump)
int bcharacterjump(lua_State *L){
	//printf("Jump called\n");
	double x,y,z;
	popvector3d(L,&x,&y,&z);
	btKinematicCharacterController *c = popCharacter(L);
	//printf("About to jump\n");
	c->jump(btVector3(x,y,z));
	//printf("Done jumping\n");
	return 0;
}

//{phys.physbox}:delete()
static int delbcharactercontroller(lua_State* L){//self
	//printf("Attempting to delete physbox\n");
	lua_getfield(L,-1,"character");//self,ud_character
	btKinematicCharacterController* r = (btKinematicCharacterController*)lua_touserdata(L,-1);//self,ud_character
	lua_pop(L,2);
	delete r->getGhostObject();
	delete r;
	return 0;
}

//{char},{v3_dir} ::
int bcharsetwalkdirection(lua_State *L){
	double x,y,z;
	popvector3d(L,&x,&y,&z);//{char}
	lua_getfield(L,-1,"character");//{char},ud_cc
	btKinematicCharacterController* cc = (btKinematicCharacterController*)lua_touserdata(L,-1);
	lua_pop(L,2);
	cc->setWalkDirection(btVector3(x,y,z));
	return 0;
}

// char:setfallspeed(n)
int bcharactersetfallspeed(lua_State *L){
	double speed = luaL_optnumber(L,-1,1);
	printf("Got number: %f\n",speed);
	lua_pop(L,1);
	btKinematicCharacterController *c = popCharacter(L);
	printf("About to set speed\n");
	c->setFallSpeed(speed);
	printf("Done setting speed\n");
	return 0;
}

// char:getmaxslope()
int bcharactergetmaxslope(lua_State *L){
	btKinematicCharacterController *c = popCharacter(L);
	btScalar s = c->getMaxSlope();
	lua_pushnumber(L,s);
	return 1;
}

// char:setmaxslope(slope)
int bcharactersetmaxslope(lua_State *L){
	btScalar s = lua_tonumber(L,-1);
	lua_pop(L,1);
	btKinematicCharacterController *c = popCharacter(L);
	c->setMaxSlope(s);
	return 0;
}

// char:setvelocityforinterval(vec3 {velocity},number interval)
int bcharactersetvelocityforinterval(lua_State *L){
	double interval = lua_tonumber(L,-1);
	lua_pop(L,1);
	double x,y,z;
	popvector3d(L,&x,&y,&z);
	btKinematicCharacterController *c = popCharacter(L);
	c->setVelocityForTimeInterval(btVector3(x,y,z),interval);
	return 0;
}

extern const luaL_reg bcharactercontroller_m[] = {
	{"setwalkdir",          bcharsetwalkdirection},
	{"remove",              delbcharactercontroller},
	{"getvelocity",         bcharactergetvelocity},
	{"setvelocity",         bcharactersetvelocity},
	{"setgravity",          bcharactersetgravity},
	{"getpos",              bcharactergetpos},
	{"setpos",              bcharactersetpos},
	{"onground",            bcharacteronground},
	{"jump",                bcharacterjump},
	{"setfallspeed",        bcharactersetfallspeed},
	{"getmaxslope",         bcharactergetmaxslope},
	{"setmaxslope",         bcharactersetmaxslope},
	{"setvelocityforinterval",bcharactersetvelocityforinterval},
	{0, 0},
};

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

	luaL_newmetatable(L, "phys.charactercontroller");//{phys.characontroller}
	lua_newtable(L);//{phys.charcontroller},{}
	luaL_register(L,NULL,bcharactercontroller_m);//{phys.charcontroller},{}
	lua_setfield(L,-2,"__index");//{phys.charcontroller}

	lua_pop(L,1);//

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

	lua_pop(L,1);
}