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

extern btDiscreteDynamicsWorld* World;
extern std::list<btRigidBody*> Objects;
/*
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_btRigidBody :: ({v3 size}, {v3 origin}, double mass)
void makenewbphysbox(lua_State* 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);

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

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

	// Create the rigid body object
	btRigidBody* rigidbody = new btRigidBody(mass, motionstate, shape, localinertia);
	if(!rigidbody){
		//printf("No rigidbody\n");
	}
    
	// Add it to the world
	World->addRigidBody(rigidbody);
	//printf("Added rigid body to world: %p\n",World);
	Objects.push_back(rigidbody);

	lua_pushlightuserdata(L,rigidbody);//ud_rigidbody
}

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

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

  	return 1;
}

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

	return 0;
}

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

	lua_getfield(L,-1,"rigidbody");//self,ud_rigidbody
	btRigidBody* i = (btRigidBody*)lua_touserdata(L,-1);//self
	btMotionState* ms = i->getMotionState();
	btTransform bt;
	ms->getWorldTransform(bt);

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

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

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

	return 1;
}

static const luaL_reg bphysbox_m[] = {
  {"getpos",               bphysgetpos},
  {"setpos",               bphyssetpos},
  {"delete", 		   delbphysbox},
  {0, 0},
};

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

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

	lua_pop(L,1);//

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

	lua_pop(L,1);
}