extern "C" {
  #include <lua.h>
  #include <lauxlib.h>
  #include <lualib.h>
}
#include <btBulletDynamicsCommon.h>
#include <shared/lua_api/common.hpp>
#include <shared/phys/physcommon.hpp>

/***
@module phys
*/


/*Physics things from lua have the form of:
{
	type = "rigidbody"
	collider = btRigidBody,
	node = ISceneNode, --optional, on client
}
*/
btRigidBody* popRigidBody(lua_State *L){
	lua_getfield(L,-1,"type");//{rigidbody},"rigidbody"
	if(lua_isnil(L,-1)){
		printf("Could not get type\n");
		lua_pushstring(L,"Tried to call a rigidbody method with something that did not have a \"type\"");
		lua_error(L);
	}
	const char *s = lua_tostring(L,-1);
	if(strcmp(s,"rigidbody") != 0){
		printf("Tried to pop rigidbody when it was not a rigidbody!\n");
		lua_pushstring(L,"Tried to call a rigidbody method on a ");
		lua_pushstring(L,s);
		lua_concat(L,2);
		lua_error(L);
	}
	lua_getfield(L,-2,"collider");//{rigidbody},"rigidbody",ud_rigidbody
	if(lua_isnil(L,-1)){
		printf("Failed to get a \"collider\" field\n");
		lua_pushstring(L,"Rigid body object was not set up correctly\n");
		lua_error(L);
	}
	btRigidBody *r = (btRigidBody*)lua_touserdata(L,-1);
	lua_pop(L,3);
	return r;
}
/***
Sets the direction of gravity on this object.
@function rigidbody:setgravity({x,y,z})
@tparam vector3d direction The direction to make gravity point
*/
//rigidbody:setgravity({x,y,z})
int setgravity(lua_State *L){
	double x,y,z;
	popvector3d(L,&x,&y,&z);

	btRigidBody *r = popRigidBody(L);
	btVector3 v = btVector3(x,y,z);
	r->setGravity(v);

	return 0;
}

/***
Gets the direction of gravity on this object.
@function rigidbody:getgravity()
@treturn vector3d The direction of gravity on this object.
*/
//rigidbody:getgravity()
int getgravity(lua_State *L){
	btRigidBody *r = popRigidBody(L);

	btVector3 v = r->getGravity();
	pushvector3d(L,v.x(),v.y(),v.z());

	return 0;
}

/***
Apply force at a reletive offset.
@function rigidbody:applyforce(direction, offset = {0,0,0})
@tparam vector3d direction The direction of the force to apply
@tparam vector3d offset The offset from the center of gravity to apply the force
*/
//rigidbody:applyforce({x,y,z}[,{rx,ry,rz}])
int applyforce(lua_State *L){
	double rx,ry,rz;
	rx = 0;
	ry = 0;
	rz = 0;
	if(lua_gettop(L) > 2){
		popvector3d(L,&rx,&ry,&rz);//{phys},{x,y,z}
	}

	double x,y,z;
	popvector3d(L,&x,&y,&z);//{phys}

	btRigidBody *r = popRigidBody(L);

	btVector3 v = btVector3(x,y,z);
	btVector3 o = btVector3(rx,ry,rz);

	r->applyForce(v,o);
	
	return 0;
}

/***
Gets the damping applied to this rigidbody
@function rigidbody:getldamping()
@treturn number damping The ammount of damping applied to the object's momentum
*/
//rigidbody:getldamping()
int getlineardamping(lua_State *L){
	btRigidBody *r = popRigidBody(L);

	double damp = r->getLinearDamping();
	lua_pushnumber(L,damp);

	return 1;
}

/***
Sets the angular factor of the rigidbody
TODO:What does this actually do?
@function rigidbody:setangfactor(vec3 dir)
@tparam vector3 dir The direction to set the angular factor
*/
int setangfactor(lua_State *L){
	double x,y,z;
	popvector3d(L,&x,&y,&z);
	btRigidBody *r = popRigidBody(L);
	r->setAngularFactor(btVector3(x,y,z));
	return 0;
}

/***
Gets the angular damping applied to this rigidbody
@function rigidbody:getadamping()
@treturn number damping The ammount of damping applied to angular momentum
*/
//rigidbody:getadamping()
int getangulardamping(lua_State *L){
	btRigidBody *r = popRigidBody(L);

	double damp = r->getAngularDamping();
	lua_pushnumber(L,damp);

	return 1;
}

/***
Gets the velocity of this object
@function rigidbody:getvelocity()
@treturn vector3 The velocity in each direction
*/
//rigidbody:getvelocity()
int getvelocity(lua_State *L){
	btVector3 vel;
	btRigidBody *r = popRigidBody(L);

	vel = r->getLinearVelocity();
	pushvector3d(L,(double)vel.x(),(double)vel.y(),(double)vel.z());

	return 1;
}

/***
Sets the velocity of this object
@function rigidbody:setvelocity()
@tparam vector3d direction The ammount on each axis to set the velocity of this object.
*/
//rigidbody:setvelocity({x,y,z})
int setvelocity(lua_State *L){
	double x,y,z;
	popvector3d(L,&x,&y,&z);
	btVector3 newvel = btVector3(x,y,z);

	btRigidBody *r = popRigidBody(L);

	r->setLinearVelocity(newvel);

	return 0;
}


/***
Sets the damping of this object.
@function rigidbody:setdamping(damping,angular_damping)
@tparam number damping The ammount of damping the object should put on it's movement.
@tparam number angular_damping The ammount of damping the object should put on it's angular momentum
*/
//rigidbody:setdamping(lineardamping, angulardamping)
int setdamping(lua_State *L){
	double adamp,ldamp;
	adamp = lua_tonumber(L,-1);
	ldamp = lua_tonumber(L,-2);
	lua_pop(L,2);

	btRigidBody *r = popRigidBody(L);

	r->setDamping(adamp,ldamp);

	return 0;
}


/***
Sets flags on this rigidbody
@function rigidbody:setflags(flags)
@tparam number flags
*/
int setflags(lua_State *L){
	int flags = lua_tonumber(L,-1);
	lua_pop(L,1);

	btRigidBody *r = popRigidBody(L);

	r->setFlags(flags);

	return 0;
}

/***
Apply an impulse to the rigidboy
@function rigidbody:applyimpulse(vec3 impulse[, vec3 offset])
@tparam vector3 impulse The direction to apply the impulse in
@tparam? vector3 offset The offset from the center to apply the impulse, default = {0, 0, 0}
*/
//applyimpuse(self,{pos},{off})
int applyimpulse(lua_State *L){
	printf("Apply impulse called...\n");
	int nargs = lua_gettop(L);
	double x,y,z,ox,oy,oz;
	if(nargs > 2){
		popvector3d(L,&ox,&oy,&oz);
	}else{
		ox = 0;
		oy = 0;
		oz = 0;
	}
	popvector3d(L,&x,&y,&z);
	btRigidBody *r = popRigidBody(L);
	r->applyImpulse(btVector3(x,y,z),btVector3(ox,oy,oz));
	return 0;
}



/*
A callback used to detect collisions
*/
//class BContactResult : public ContactResultCallback
//{
	//public:
		//~BContactResult(){
			//printf("Contact result being destroyed\n");
		//}
		//bool needsCollision(){
			//return true;
		//}
		//btScalar addSingleResult(btManifoldPoint point,
					 //btCollisionObjectWrapper *wrap1, int part1, int index1,
					 //btCollisionObjectWrapper *wrap2, int part2, int index2
					//){
			//printf("Got single result\n");
			//return 1;
		//}
//};

//rigidbody:contacttest()
//int testcontact(lua_State *L){//{rigibody}
	//printf("Testing contact\n");
	//btRigidBody *r = popRigidBody(L);//
	//ContactResultCallback *cr = new BContactResult();
	//World->contactTest(r,cr);
	//return 0;
//}

extern const luaL_reg brigidbody_m[] = {
	{"setgravity",       setgravity},
	{"applyforce",       applyforce},
	{"applyimpulse",     applyimpulse},
	{"getldamping",      getlineardamping},
	{"getadamping",      getangulardamping},
	{"setdamping",       setdamping},
	//{"activate",         activate}, --moved to bcollider_m
	{"getvelocity",      getvelocity},
	{"setvelocity",      setvelocity},
	{"setangfactor",     setangfactor},
	{"setflags",         setflags},
	//{"testcontact",      testcontact},
	{NULL, NULL}
};