/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include "squirrel.h"
		#include "binding_helpers.h"
		#include "physic/physic_constraint.h"
		#include "physic/physic_world.h"
		#include "scene3d/mconstraint.h"
#include "scene3d/scene.h"
#include "math/matrix4.h"
#include "math/vector.h"

		using namespace GS::S3D;
		using namespace GS::Script;


//------------------------------------------------------------------------------
SQInteger			SceneAddConstraint(HSQUIRRELVM vm)
{
	__SQ_GETSTART(2)
	__SQ_GETSAFEPTR(s, Scene, typetag_Scene3d)
	__SQ_GETSTRING(name)
	if	(!s->physic_world)
		return sq_throwerror(vm, "Cannot create constraint, no physic world in scene.");
	MConstraint *c = new MConstraint(s->physic_world->NewConstraint());
	if	(!c)
		return sq_throwerror(vm, "Failed to allocate constraint.");
	c->name = name;
	s->AddItem(c, true);

	__SQ_GETEND
	__SQ_RETURNSAFEPTR(c, typetag_Constraint)
}
SQInteger			SceneAddPointConstraint(HSQUIRRELVM vm)
{
	__SQ_GETSTART(6)
	__SQ_GETSAFEPTR(s, Scene, typetag_Scene3d)
	__SQ_GETSTRING(name)
	__SQ_GETSAFEPTR(a, MItem, typetag_Item)
	__SQ_GETSAFEPTR(b, MItem, typetag_Item)
	__SQ_GETVECTOR(pivot_a)
	__SQ_GETVECTOR(pivot_b)

	if	(!s->physic_world)
		return sq_throwerror(vm, "Cannot create constraint, no physic world in scene.");
	MConstraint *c = new MConstraint(s->physic_world->NewConstraint());
	if	(!c)
		return sq_throwerror(vm, "Failed to allocate constraint.");
	c->name = name;
	s->AddItem(c, true);

	if	(!a->physic_item || !b->physic_item)
		return sq_throwerror(vm, "Item has no physics component.");

	c->desc.type = PhysicConstraintDesc::TypePoint;
	c->desc.item_a = a;
	c->desc.item_b = b;
	c->desc.pivot_a = GS::Matrix4::TranslationMatrix(pivot_a);
	c->desc.pivot_b = GS::Matrix4::TranslationMatrix(pivot_b);

	c->Setup(s->physic_world);

	__SQ_GETEND
	__SQ_RETURNSAFEPTR(c, typetag_Constraint)
}
SQInteger			SceneAddPointConstraintHinge(HSQUIRRELVM vm)
{
	__SQ_GETSTART(6)
		__SQ_GETSAFEPTR(s, Scene, typetag_Scene3d)
		__SQ_GETSTRING(name)
		__SQ_GETSAFEPTR(a, MItem, typetag_Item)
		__SQ_GETSAFEPTR(b, MItem, typetag_Item)
		__SQ_GETVECTOR(pivot_a)
		__SQ_GETVECTOR(pivot_b)

		if	(!s->physic_world)
			return sq_throwerror(vm, "Cannot create constraint, no physic world in scene.");
	MConstraint *c = new MConstraint(s->physic_world->NewConstraint());
	if	(!c)
		return sq_throwerror(vm, "Failed to allocate constraint.");
	c->name = name;
	s->AddItem(c, true);

	if	(!a->physic_item || !b->physic_item)
		return sq_throwerror(vm, "Item has no physics component.");

	c->desc.type = PhysicConstraintDesc::TypeHinge;
	c->desc.item_a = a;
	c->desc.item_b = b;
	c->desc.pivot_a = GS::Matrix4::TranslationMatrix(pivot_a);
	c->desc.pivot_b = GS::Matrix4::TranslationMatrix(pivot_b);

	c->Setup(s->physic_world);

	__SQ_GETEND
		__SQ_RETURNSAFEPTR(c, typetag_Constraint)
}
SQInteger			ConstraintEnable(HSQUIRRELVM vm)
{
	__SQ_GETSTART(2)
	__SQ_GETSAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_GETBOOL(b)
	__SQ_GETEND
	if	(c->physic_data.IsValid())
		c->physic_data->Enable(asbool(b));
	__SQ_RETURN
}
SQInteger			ConstraintGetPivotA(HSQUIRRELVM vm)
{
	__SQ_GETSINGLESAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_RETURNVECTOR(c->desc.pivot_a.GetRow(3))
}
SQInteger			ConstraintGetPivotB(HSQUIRRELVM vm)
{
	__SQ_GETSINGLESAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_RETURNVECTOR(c->desc.pivot_b.GetRow(3))
}
SQInteger			ConstraintSetPivotA(HSQUIRRELVM vm)
{
	__SQ_GETSTART(2)
	__SQ_GETSAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_GETVECTOR(pivot)
	__SQ_GETEND
	c->desc.pivot_a = GS::Matrix4::TranslationMatrix(pivot);
	c->physic_data->SetPivotA(c->desc.pivot_a);
	__SQ_RETURN
}
SQInteger			ConstraintSetPivotB(HSQUIRRELVM vm)
{
	__SQ_GETSTART(2)
	__SQ_GETSAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_GETVECTOR(pivot)
	__SQ_GETEND
	c->desc.pivot_b = GS::Matrix4::TranslationMatrix(pivot);
	c->physic_data->SetPivotB(c->desc.pivot_b);
	__SQ_RETURN
}
#include "physic_bullet/bullet_constraint.h"
SQInteger			ConstraintSetLimit(HSQUIRRELVM vm)
{
	__SQ_GETSTART(6)
		__SQ_GETSAFEPTR(c, MConstraint, typetag_Constraint)
		__SQ_GETFLOAT(low)
		__SQ_GETFLOAT(high)
		__SQ_GETFLOAT(softness)
		__SQ_GETFLOAT(biasfactor)
		__SQ_GETFLOAT(relaxationFactor)
		__SQ_GETEND
		((BulletConstraint*)(c->physic_data.c_ptr()))->setLimitHinge(low, high, softness, biasfactor, relaxationFactor);
	__SQ_RETURN
}

//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
SQInteger			ConstraintGetItem(HSQUIRRELVM vm)
{
	__SQ_GETSINGLESAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_RETURNSAFEPTR((MItem *)c, typetag_Item)
}
SQInteger			ConstraintSetItemA(HSQUIRRELVM vm)
{
	__SQ_GETSTART(2)
	__SQ_GETSAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_GETSAFEPTR(i, MItem, typetag_Item)
	__SQ_GETEND
	c->desc.item_a = i;
	__SQ_RETURN
}
SQInteger			ConstraintSetItemB(HSQUIRRELVM vm)
{
	__SQ_GETSTART(2)
	__SQ_GETSAFEPTR(c, MConstraint, typetag_Constraint)
	__SQ_GETSAFEPTR(i, MItem, typetag_Item)
	__SQ_GETEND
	c->desc.item_b = i;
	__SQ_RETURN
}
//------------------------------------------------------------------------------
#include "script_squirrel/mmf.h"

// See TELEMETRY_DATA.flags
#define MMF_CONFIG_EXPLICIT_LOCAL_VEL 1
#define MMF_CONFIG_EXPLICIT_LOCAL_ACCEL 2
#define MMF_CONFIG_EXPLICIT_GLOBAL_ACCEL 4
#define MMF_CONFIG_EXPLICIT_ACCEL_EXCLUDES_GRAVITY 8

// See TELEMETRY_DATA.flags
#define CONFIG_EXPLICIT_LOCAL_VEL				(1 << 0)
#define CONFIG_EXPLICIT_LOCAL_ACCEL				(1 << 1)
#define CONFIG_EXPLICIT_ACCEL_EXCLUDES_GRAVITY	(1 << 3)
#define CONFIG_ROW_ORDERED_MATRIX				(1 << 4)

// See TELEMETRY_DATA.axis
#define AXIS_X_UP								(1 << 0)
#define AXIS_X_DOWN								(1 << 1)
#define AXIS_X_NORTH							(1 << 2)
#define AXIS_X_SOUTH							(1 << 3)
#define AXIS_X_EAST								(1 << 4)
#define AXIS_X_WEST								(1 << 5)
#define AXIS_Y_UP								(1 << 8)
#define AXIS_Y_DOWN								(1 << 9)
#define AXIS_Y_NORTH							(1 << 10)
#define AXIS_Y_SOUTH							(1 << 11)
#define AXIS_Y_EAST								(1 << 12)
#define AXIS_Y_WEST								(1 << 13)
#define AXIS_Z_UP								(1 << 16)
#define AXIS_Z_DOWN								(1 << 17)
#define AXIS_Z_NORTH							(1 << 18)
#define AXIS_Z_SOUTH							(1 << 19)
#define AXIS_Z_EAST								(1 << 20)
#define AXIS_Z_WEST								(1 << 21)


struct TELEMETRY_DATA
{
	unsigned int flags;
	unsigned int axis;
	float accel[3];
	float vel[3];
	float rotationMatrix[3][3];
	DWORD packetTimeMillis;
};
/*
struct SIMPHYNITYMMF
{
unsigned char flags;
DWORD packetTime;
float telemetryMatrix[16];
float velocity[3];
float accel[3];
};*/
struct SIMPHYNITYMMF
{
	DWORD		packetTime;
	float 		telemetryMatrix[16];
	float 		globalVelocity[3];
};


//-------------------------------------------------------
SQInteger			CreateMMF(HSQUIRRELVM vm)
//-------------------------------------------------------
{
	__SQ_RETURNSAFEPTR(new CMMF(_T("$SIMPHYNITYTELEM$"), sizeof(TELEMETRY_DATA), _T("$SIMPHYNITYTELEMMUTEX$")), typetag_Item)
//	__SQ_RETURNSAFEPTR(new CMMF(_T("$SIMPHYNITYTELEM$"), sizeof(SIMPHYNITYMMF), _T("$SIMPHYNITYTELEMMUTEX$")), typetag_Item)
}
#define	__SQ_GETPHYSICITEM(__I__) PhysicItem *iphysic = (__I__)->physic_item.c_ptr(); if (!iphysic) return sq_throwerror(vm, "No physics found, did you setup this item?");
//-------------------------------------------------------
SQInteger			UpdateMMF(HSQUIRRELVM vm)
//-------------------------------------------------------
{
	__SQ_GETSTART(3)
	__SQ_GETSAFEPTR(mmf, CMMF, typetag_Item)
	__SQ_GETSAFEPTR(item, MItem, typetag_Item)
	__SQ_GETFLOAT(Physic_T)
	__SQ_GETEND

	__SQ_GETPHYSICITEM(item)

	GS::Matrix4 item_matrix;
	iphysic->GetMatrix(item_matrix);
	item_matrix = item->GetBaseItem()->GetMatrix();

	GS::Vector4 position;
	GS::Vector4 scale;
	GS::Matrix3 rotation;
	item_matrix.Decompose(&position, &scale, &rotation);

//	GS::Vector4 velocity_vec = (item && item->isActive() ? iphysic->GetLinearVelocity() : GS::Vector4(0, 0, 0));
	GS::Vector4 velocity_vec = (item && item->isActive() ? iphysic->GetLinearVelocity() : GS::Vector4(0, 0, 0));
/*
	
	SIMPHYNITYMMF m_Telem;
	m_Telem.packetTime = Physic_T*1000.0f; // Current physics time.
	memcpy(m_Telem.telemetryMatrix, item_matrix.m, sizeof(m_Telem.telemetryMatrix)); // Current rotation, position.
	memcpy(m_Telem.globalVelocity, ((float *)&(velocity_vec.x)), sizeof(m_Telem.globalVelocity)); // Global vel XYZ.
	mmf->Write(&m_Telem);
*/

	TELEMETRY_DATA m_Telem;
	m_Telem.packetTimeMillis = Physic_T*1000.0f; // Current physics time.
	m_Telem.axis = AXIS_X_EAST | AXIS_Y_UP | AXIS_Z_NORTH;
	m_Telem.flags = CONFIG_EXPLICIT_LOCAL_VEL | CONFIG_EXPLICIT_ACCEL_EXCLUDES_GRAVITY | CONFIG_ROW_ORDERED_MATRIX;
	memcpy(m_Telem.rotationMatrix, rotation.m, sizeof(m_Telem.rotationMatrix)); // Current rotation
	memcpy(m_Telem.vel, ((float *)&(velocity_vec.x)), sizeof(m_Telem.vel)); // Global vel XYZ.
	m_Telem.accel[0] = 0; m_Telem.accel[1] = 0; m_Telem.accel[2] = 0;
	mmf->Write(&m_Telem);

 
	__SQ_RETURN
}


//--------------------------------------------------------------
struct ENGINE_TELEMETRY_DATA
{
	char package[32768];
};

SQInteger			CreateEngineMMF(HSQUIRRELVM vm)
//-------------------------------------------------------
{
	__SQ_RETURNSAFEPTR(new CMMF(_T("$DevelterInnovationSimulateur$"), sizeof(ENGINE_TELEMETRY_DATA), _T("$DevelterInnovationSimulateurMUTEX$")), typetag_Item)
}
//-------------------------------------------------------
SQInteger			UpdateEngineMMF(HSQUIRRELVM vm)
//-------------------------------------------------------
{
	__SQ_GETSTART(2)
	__SQ_GETSAFEPTR(mmf, CMMF, typetag_Item)
	__SQ_GETSTRING(package)
	__SQ_GETEND

	GS::String spackage(package);	

	if (spackage.Size() > 32768)
		return sq_throwerror(vm, "UpdateEngineMMF: package bigger than 16384 bits.");

	ENGINE_TELEMETRY_DATA m_Telem;
	memset(m_Telem.package, 0, 32768);
	memcpy(m_Telem.package, spackage.c_str(), spackage.Size());
	mmf->Write(&m_Telem);

	__SQ_RETURN
}
//------------------------------------------------------------------------------
void		RegisterPhysicBinding(HSQUIRRELVM vm)
{
/*#
	Topic: Physic
	Type: Constraint
	Desc: Physic world functions. For item related physic functions please refer to the Item topic.
	Related: Item
#*/

	/*#
	Func: CreateMMF
	Proto: MMF:void
	Desc: Create mmf object for simu purpose.
	#*/
	sq_register(vm, CreateMMF, "CreateMMF", _SC("."));
	/*#	
	Func: UpdateMMF
	Proto: void:MMF, ,
	Desc: update the mmf with the object rotation and velocity.
	#*/
	sq_register(vm, UpdateMMF, "UpdateMMF", _SC(".xxf"));

	/*#
	Func: CreateEngineMMF
	Proto: MMF:void
	Desc: Create mmf object for simu purpose.
	#*/
	sq_register(vm, CreateEngineMMF, "CreateEngineMMF", _SC("."));
	/*#
	Func: UpdateEngineMMF
	Proto: void:MMF
	Desc: update the mmf with the json values
	#*/
	sq_register(vm, UpdateEngineMMF, "UpdateEngineMMF", _SC(".xs"));
/*#
	Section: ConstraintManagement
	Desc: Constraint management functions
#*/
	/*#
		Func: SceneAddPointConstraint
		Proto: Constraint:Scene,string name, Item a,Item b,Vector pivot_a,Vector pivot_b
		Desc: Create a new point constraint between two items. The pivot position is in item space. The null item can be set as item B to specify an unmovable world space hook.
	#*/
	sq_register(vm, SceneAddPointConstraint, "SceneAddPointConstraint", _SC(".xsxxxx"));
	/*#
		Func: SceneAddPointConstraintHinge
		Proto: Constraint:Scene,string name, Item a,Item b,Vector pivot_a,Vector pivot_b
		Desc: Create a new point constraint between two items. The pivot position is in item space. The null item can be set as item B to specify an unmovable world space hook.
	#*/
	sq_register(vm, SceneAddPointConstraintHinge, "SceneAddPointConstraintHinge", _SC(".xsxxxx"));
	
	/*#
		Func: ConstraintEnable
		Proto: void:Constraint,bool
		Desc: Enable/disable constraint.
	#*/
	sq_register(vm, ConstraintEnable, "ConstraintEnable", _SC(".xb"));

/*#
	Section: ConstraintConfiguration
	Desc: Constraint configuration functions
#*/
	/*#
		Func: ConstraintGetPivotA
		Proto: Vector:Constraint
		Desc: Get the constraint item A pivot vector.
	#*/
	sq_register(vm, ConstraintGetPivotA, "ConstraintGetPivotA", _SC(".x"));
	/*#
		Func: ConstraintGetPivotB
		Proto: Vector:Constraint
		Desc: Get the constraint item B pivot vector.
	#*/
	sq_register(vm, ConstraintGetPivotB, "ConstraintGetPivotB", _SC(".x"));
	/*#
		Func: ConstraintSetPivotA
		Proto: void:Constraint,Vector pivot
		Desc: Set the constraint item A pivot vector.
	#*/
	sq_register(vm, ConstraintSetPivotA, "ConstraintSetPivotA", _SC(".xx"));
	/*#
		Func: ConstraintSetPivotB
		Proto: void:Constraint,Vector pivot
		Desc: Set the constraint item B pivot vector.
	#*/
	sq_register(vm, ConstraintSetPivotB, "ConstraintSetPivotB", _SC(".xx"));
	/*#
	Func: ConstraintSetLimit
	Proto: void:Constraint,float low, float high, float softness, float biasfactor, float relaxationfactor
	Desc: Set the constraint limit.
	#*/
	sq_register(vm, ConstraintSetLimit, "ConstraintSetLimit", _SC(".xfffff"));
	
	/*#
		Func: ConstraintGetItem
		Proto: Item:Constraint
		Desc: Get constraint item.
	#*/
	sq_register(vm, ConstraintGetItem, "ConstraintGetItem", _SC(".x"));
	/*#
		Func: ConstraintSetItemA
		Proto: void:Constraint,Item
		Desc: Set constraint item A.
	#*/
	sq_register(vm, ConstraintSetItemA, "ConstraintSetItemA", _SC(".xx"));
	/*#
		Func: ConstraintSetItemB
		Proto: void:Constraint,Item
		Desc: Set constraint item B.
	#*/
	sq_register(vm, ConstraintSetItemB, "ConstraintSetItemB", _SC(".xx"));

	// Push defines.
	sq_pushroottable(vm);

	sq_pop(vm, 1);
}
//------------------------------------------------------------------------------