#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <memory>
#include <map>
#include <functional>
extern "C" {
  #include <lua.h>
  #include <lauxlib.h>
  #include <lualib.h>
}
#include <btBulletDynamicsCommon.h>
#include <irrlicht.h>
#include "../gameparts.hpp"
#include "bphysbox.hpp"
#include "bphysmodel.hpp"
#include "../scene/igeneric.hpp"

using namespace irr;
using namespace scene;
using namespace core;
using namespace video;

extern IrrlichtDevice* device;

extern btDiscreteDynamicsWorld* World;
extern core::list<btRigidBody*> Objects;

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

/*
static LISceneNode* checkismesh(lua_State* L){
  return checkismesh(L,1);
}
*/

//iscenecamera.new(Vector position, Vector lookat, parrent)
// {} {} 0 1
static int newbphysmodel(lua_State* L){
	printf("Createing bphysbox!\n");
	int nargs = lua_gettop(L);
	if(nargs != 3){
		printf("Incorrect # of args to create a physmodel!");
	}
	//The model for the mesh
	//const char* modelpath = luaL_optstring(L,1,"error");
	
	//Find the vector position
	lua_pushnumber(L,1);
	lua_gettable(L,-4);
	float x = lua_tonumber(L,-1);
	lua_pop(L,1);
	
	lua_pushnumber(L,2);
	lua_gettable(L,-4);
	float y = lua_tonumber(L,-1);
	lua_pop(L,1);
	
	lua_pushnumber(L,3);
	lua_gettable(L,-4);
	float z = lua_tonumber(L,-1);
	lua_pop(L,1);
	printf("Found position for phys model: %f %f %f\n",x,y,z);
	
	//Find the vector scale
	lua_pushnumber(L,1);
	lua_gettable(L,-3);
	float sx = lua_tonumber(L,-1);
	lua_pop(L,1);
	
	lua_pushnumber(L,2);
	lua_gettable(L,-3);
	float sy = lua_tonumber(L,-1);
	lua_pop(L,1);
	
	lua_pushnumber(L,3);
	lua_gettable(L,-3);
	float sz = lua_tonumber(L,-1);
	lua_pop(L,1);
	
	printf("Found scale for phys model: %f %f %f\n",sx,sy,sz);
	
	//find the model path
	const char* mpath = luaL_optstring(L,3,"error.obj");
	
	printf("I want to use model %s\n", mpath);
	
    ISceneManager* smgr = device->getSceneManager();
    IMesh* amesh = smgr->getMesh(mpath);
    IMeshBuffer* bf = amesh->getMeshBuffer(0);
    u32 ni = bf->getIndexCount();
    
    btTriangleMesh* trimesh = new btTriangleMesh();
    for(u32 i = 0; i < ni; i+=3){
        vector3df p1 = bf->getPosition(i + 0);
        vector3df p2 = bf->getPosition(i + 1);
        vector3df p3 = bf->getPosition(i + 2);
        btVector3 b1 = btVector3(p1.X,p1.Y,p1.Z);
        btVector3 b2 = btVector3(p2.X,p2.Y,p2.Z);
        btVector3 b3 = btVector3(p3.X,p3.Y,p3.Z);
        trimesh->addTriangle(b1,b2,b3);
    }
    btCollisionShape* shape = new btConvexTriangleMeshShape(trimesh,true);
	core::vector3df scale = core::vector3df(sx,sy,sz);
	btVector3 pos = btVector3(x,y,z);
	core::vector3df ipos = core::vector3df(x,y,z);
    shape->setLocalScaling(btVector3(sx,sy,sz));
	//Find the mass
	float mass = luaL_optint(L,4,0);
    printf("Found mass for physbox:%f\n",mass);

	

	// Create an Irrlicht cube
	scene::ISceneNode* Node = smgr->addMeshSceneNode(
        amesh,
        (ISceneNode*)0,
        (s32)-1,
        ipos,
        vector3df(0,0,0),
        scale
    );
	//Node->setScale(scale);
    
    printf("Added cube scene node and set it's scale\n");

    //Node->setMaterialFlag(video::EMF_WIREFRAME,true)
	//Node->setMaterialFlag(video::EMF_NORMALIZE_NORMALS, true);
    Node->setMaterialFlag(video::EMF_LIGHTING,true);
	//Node->setMaterialTexture(0, device->getVideoDriver()->getTexture("../data/wall.jpg"));

    printf("Set node's lighting stuff...\n");

	// Set the initial position of the object
	btTransform Transform;
	Transform.setIdentity();
	Transform.setOrigin(pos);
    
    printf("Created transform at pos...\n");

	// Give it a default MotionState
	btDefaultMotionState *MotionState = new btDefaultMotionState(Transform);

	// Create the shape
	// btVector3 HalfExtents(sx * 0.5f, sy * 0.5f, sz * 0.5f);
	// btCollisionShape *Shape = new btBoxShape(HalfExtents);

    printf("Created collision shape...");
    
	// Add mass
	btVector3 LocalInertia;
	shape->calculateLocalInertia(mass, LocalInertia);

	// Create the rigid body object
	btRigidBody *RigidBody = new btRigidBody(mass, MotionState, shape, LocalInertia);
    
    printf("Created rigidboxy...");

	// Store a pointer to the irrlicht node so we can update it later
	RigidBody->setUserPointer((void *)(Node));

    printf("Set user pointer");

	// Add it to the world
	World->addRigidBody(RigidBody);
    printf("Added to world");
	Objects.push_back(RigidBody);

	//Register it's callback
    printf("Everything created, makeing the lua representation\n");

	//Create it's lua representation
	LBPhysNode* pnode = (LBPhysNode*)lua_newuserdata(L, sizeof(LBPhysNode));
	int tref = luaL_ref(L,LUA_REGISTRYINDEX);
	//iguielements[lcam] = tref;
	lua_rawgeti(L,LUA_REGISTRYINDEX,tref);//Put it back on the stack since luaL_ref pops the object.

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

	//Create the struct
	pnode->n = Node;
	pnode->r = RigidBody;
	pnode->funcmap = hashmap_new();
	pnode->type = "bphysbox";

    printf("Done createing lua representation!\n");
	//Free up anything made in this function
    //free(label);

	//Put it on top and return it
  lua_rawgeti(L,LUA_REGISTRYINDEX,tref);
  return 1;
}

static int delbphysmodel(lua_State* L){
    LBPhysNode* pnode = checkisbphysmodel(L,-1);
    
    //delete pnode->r->getMotionState();
    delete pnode->r->getCollisionShape();
    delete pnode->r;
    
    return 0;
}

static int bphyssetpos(lua_State *L){
    LBPhysNode* i = checkisbphysmodel(L,1);
    btTransform bt = i->r->getWorldTransform();
    
    lua_pushnumber(L,1);
    lua_gettable(L,-2);
    f32 x = (f32)lua_tonumber(L,-1);
    lua_pop(L,1);
    
    lua_pushnumber(L,2);
    lua_gettable(L,-2);
    f32 y = (f32)lua_tonumber(L,-1);
    lua_pop(L,1);
    
    lua_pushnumber(L,3);
    lua_gettable(L,-2);
    f32 z = (f32)lua_tonumber(L,-1);
    lua_pop(L,1);
    
    btVector3 to = btVector3(x,y,z);
    printf("managed to set phys origin\n");
    bt.setOrigin(to);
    printf("managed to set phys transform\n");
    i->r->setWorldTransform(bt);
    i->r->activate();
    printf("sucess! returning from call\n");
    return 0;
}

static int bphysgetpos(lua_State *L){
    LBPhysNode* i = checkisbphysmodel(L,1);
    btTransform bt = i->r->getWorldTransform();
    btVector3 bv = bt.getOrigin();
    
    lua_createtable(L,3,0);
    
    lua_pushnumber(L,1);
    lua_pushnumber(L,bv.x());
    lua_settable(L,-3);
    
    lua_pushnumber(L,2);
    lua_pushnumber(L,bv.y());
    lua_settable(L,-3);
    
    lua_pushnumber(L,3);
    lua_pushnumber(L,bv.z());
    lua_settable(L,-3);
    
    return 1;
}

static const luaL_reg bphysmodel_f[] = {
  {"new",           newbphysbuffer},
//  {"gethandeler",   guigethandeler},
//  {"sethandeler",   guisethandeler},
  {0,0},
};

static const luaL_reg bphysmodel_m[] = {
  {"setmaterial",          iscenesetmaterial},
  {"getpos",               bphysgetpos},
  {"setpos",               bphyssetpos},
//  {"settext",       setiguitext},
//  {"remove",        removeiguielement},
  {0, 0},
};

int bphysmodel_register(lua_State* L, IrrlichtDevice* d){

    device = d;

    printf("bphysmodel registered\n");

    luaL_newmetatable(L, "phys.physmodel");

    luaL_register(L,"physmodel",bphysmodel_f);

    lua_pushstring(L,"__index");
    lua_pushstring(L,"gethandeler");
    lua_gettable(L,-3);
    lua_settable(L,-4);

    lua_pushstring(L,"__newindex");
    lua_pushstring(L,"sethandeler");
    lua_gettable(L,-3);
    lua_settable(L,-4);

    lua_pushstring(L,"__gc");
    lua_pushcfunction(L,delbphysmodel);
    lua_settable(L,-4);

    luaL_register(L, NULL, bphysmodel_m);

    return 1;
}