#include "load_phys.hpp"
#include "phys/bphysbox.hpp"
#include "phys/bhingeconstraint.hpp"
#include "phys/bcharactercontroller.hpp"
#include "phys/btaction.hpp"
#include "phys/bshape.hpp"
#include <btBulletDynamicsCommon.h>
#include <btBulletCollisionCommon.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
#include <shared/lua_api/common.hpp>

extern btDiscreteDynamicsWorld* World;

//phys.shapecast(shape,{fromx,fromy,fromz},{tox,toy,toz})
//returns {
//	hit :: boolean
//	pos :: v3
//	normal :: v3
//}
int shapecast(lua_State *L){
	double fx,fy,fz,tx,ty,tz;
	popvector3d(L,&fx,&fy,&fz);
	popvector3d(L,&tx,&ty,&tz);
	lua_getfield(L,-1,"shape");
	btBoxShape *cs = (btBoxShape*)lua_touserdata(L,-1);
	//printf("shape:%p\n",(void*)cs);
	btTransform ft = btTransform(btQuaternion(0,0,0),btVector3(fx,fy,fz));
	btTransform tt = btTransform(btQuaternion(0,0,0),btVector3(tx,ty,tz));
	btCollisionWorld::ClosestConvexResultCallback *cb = new btCollisionWorld::ClosestConvexResultCallback(ft.getOrigin(),tt.getOrigin());
	btVector3 hw, hn;
	//hw = cb->m_hitPointWorld;
	//hn = cb->m_hitNormalWorld;
	//printf("before getting results\n\tHasHit:%d\n\tHit:%f,%f,%f\n\tNor:%f,%f,%f\n",cb->hasHit() ? 1 : 0,hw.x(),hw.y(),hw.z(),hn.x(),hn.y(),hn.z());
	World->convexSweepTest(cs,ft,tt,*cb,1.f);
	hw = cb->m_hitPointWorld;
	//Okay, turns out m_hitNormalWorld is not the normal of either of the shapes (although it's usually close):
	//https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=3538
	hn = cb->m_hitNormalWorld;
	//printf("Got sweep results\n\tHasHit:%d\n\tHit:%f,%f,%f\n\tNor:%f,%f,%f\n",cb->hasHit() ? 1 : 0,hw.x(),hw.y(),hw.z(),hn.x(),hn.y(),hn.z());
	lua_newtable(L);//{}
	lua_pushboolean(L,cb->hasHit() ? 1 : 0);
	lua_setfield(L,-2,"hit");
	pushvector3d(L,hw.x(),hw.y(),hw.z());
	lua_setfield(L,-2,"pos");
	pushvector3d(L,hn.x(),hn.y(),hn.z());
	lua_setfield(L,-2,"normal");
	delete cb;
	return 1;
}

//phys.aabbcast(v3 from, v3 to, v3 mins, v3 maxs)
int aabbcast(lua_State *L){
	double sx,sy,sz,ex,ey,ez,minx,miny,minz,maxx,maxy,maxz;
	popvector3d(L,&maxx,&maxy,&maxz);
	popvector3d(L,&minx,&miny,&minz);
	popvector3d(L,&ex,&ey,&ez);
	popvector3d(L,&sx,&sy,&sz);
	btVector3 normal;
	btScalar param = btScalar(1.);
	bool out = btRayAabb(btVector3(sx,sy,sz),btVector3(ex,ey,ez),btVector3(minx,miny,minz),btVector3(maxx,maxy,maxz),param,normal);
	printf("aabbcast:\n\thashit:%d\n\tparam:%f\n\tnormal:%f,%f,%f\n",out ? 1 : 0, param, normal.x(),normal.y(),normal.z());
	lua_pushboolean(L,out?1:0);
	return 1;
}

void load_physfuncs(lua_State* L){
	printf("Loading phys things\n");
	lua_newtable(L);//{}
	lua_setglobal(L,"phys");//
	lua_getglobal(L,"phys");//{phys}
	lua_pushcfunction(L,shapecast);
	lua_setfield(L,-2,"shapecast");
	lua_pushcfunction(L,aabbcast);
	lua_setfield(L,-2,"aabbcast");
	lua_newtable(L);//{phys},{}
	lua_setfield(L,-2,"colliders");//{phys}
	set_const(L,BT_DISABLE_WORLD_GRAVITY);
	set_const(L,BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT);
	set_const(L,BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD);
	set_const(L,BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY);
	bphysbox_register(L);
	bhingeconstraint_register(L);
	bcharactercontroller_register(L);
	printf("About to allow makeaction\n");
	baction_register(L);
	bshape_register(L);
	printf("Done registering makeaction\n");
}