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


		#include "physic_bullet/bullet_constraint.h"
		#include "physic_bullet/bullet_item.h"
		#include "scene3d/mitem.h"

		using namespace GS;
		using namespace GS::S3D;

void BulletConstraint::setLimitHinge(float low, float high, float _softness, float _biasFactor, float _relaxationFactor)
{
	if (!constraint)
		return;

	switch (type)
	{
		case PhysicConstraintDesc::TypeHinge:
			((btHingeConstraint*)constraint)->setLimit(low, high, _softness, _biasFactor, _relaxationFactor);
		break;
	}
}
//------------------------------------------------------------------------------
void				BulletConstraint::SetPivotA(const Matrix4 &pivot)
{
	if	(!constraint)
		return;

	switch (type)
	{
		case PhysicConstraintDesc::TypePoint:
		{
			Vector4 p = pivot.GetRow(3);
			if	(item_a.IsValid())
				p -= ((BulletPhysicItem *)item_a.c_ptr())->GetCenter();

			btVector3 bt_pivot(p.x, p.y, p.z);
			((btPoint2PointConstraint *)constraint)->setPivotA(bt_pivot);
		}
		break;

		default: break;
	}
}
void				BulletConstraint::SetPivotB(const Matrix4 &pivot)
{
	if	(constraint)
		switch (type)
		{
			case PhysicConstraintDesc::TypePoint:
			{
				Vector4 p = pivot.GetRow(3);
				if	(item_b.IsValid())
					p -= ((BulletPhysicItem *)item_b.c_ptr())->GetCenter();

				btVector3 bt_pivot(p.x, p.y, p.z);
				((btPoint2PointConstraint *)constraint)->setPivotB(bt_pivot);
			}
			break;

			default: break;
		}
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				BulletConstraint::Enable(bool b)
{
	if	(constraint)
		constraint->setEnabled(b);
}
bool				BulletConstraint::SetupConstraint(const PhysicConstraintDesc &desc)
{
	DeleteConstraint();

	// Get constraint items.
	item_a = desc.item_a.IsValid() ? desc.item_a->physic_item.c_ptr() : NULL;
	item_b = desc.item_b.IsValid() ? desc.item_b->physic_item.c_ptr() : NULL;

	BulletPhysicItem *bullet_item_a = (BulletPhysicItem *)item_a.c_ptr(),
					  *bullet_item_b = (BulletPhysicItem *)item_b.c_ptr();

	btRigidBody *rigid_body_a = bullet_item_a ? bullet_item_a->rigid_body.c_ptr() : NULL,
				*rigid_body_b = bullet_item_b ? bullet_item_b->rigid_body.c_ptr() : NULL;

	if	(!rigid_body_a && !rigid_body_b)
		return false;

	// Create constraint.
	type = desc.type;

	switch (type)
	{
		case PhysicConstraintDesc::TypePoint:
		{
			Vector4 np_a = desc.pivot_a.GetRow(3), np_b = desc.pivot_b.GetRow(3);

			if	(bullet_item_a)
				np_a -= bullet_item_a->GetCenter();
			if	(bullet_item_b)
				np_b -= bullet_item_b->GetCenter();

			btVector3 btp_a(np_a.x, np_a.y, np_a.z), btp_b(np_b.x, np_b.y, np_b.z);

			if	(rigid_body_a && rigid_body_b)
				constraint = new btPoint2PointConstraint(*rigid_body_a, *rigid_body_b, btp_a, btp_b);
			if	(rigid_body_a && !rigid_body_b)
				constraint = new btPoint2PointConstraint(*rigid_body_a, btp_a);

		//	constraint->setParam(BT_CONSTRAINT_ERP, 0.8);
		//	constraint->setParam(BT_CONSTRAINT_CFM, 0);
		}
		break;
		case PhysicConstraintDesc::TypeHinge:
			{
				Vector4 np_a = desc.pivot_a.GetRow(3), np_b = desc.pivot_b.GetRow(3);

				if	(bullet_item_a)
					np_a -= bullet_item_a->GetCenter();
				if	(bullet_item_b)
					np_b -= bullet_item_b->GetCenter();

				btVector3 btp_a(np_a.x, np_a.y, np_a.z), btp_b(np_b.x, np_b.y, np_b.z);

				if	(rigid_body_a && rigid_body_b)
					constraint = new btHingeConstraint(*rigid_body_a, *rigid_body_b, btp_a, btp_b, btVector3(0,0,1), btVector3(0,0,1));
				if	(rigid_body_a && !rigid_body_b)
					constraint = new btHingeConstraint(*rigid_body_a, btp_a, btVector3(0,1,0));
			//	((btHingeConstraint*)constraint)->setLimit(0, 0);
			//	constraint->setParam(BT_CONSTRAINT_STOP_CFM, 0);
			//	constraint->setParam(BT_CONSTRAINT_CFM, 0);
			//	constraint->setParam(BT_CONSTRAINT_STOP_ERP, 0.8);
			//	constraint->setParam(BT_CONSTRAINT_ERP, 0.8);
			}
			break;
		

		default: break;
	}

	if	(constraint)
		world->addConstraint(constraint);

	return true;
}
void				BulletConstraint::DeleteConstraint()
{
	if	(constraint)
	{
		world->removeConstraint(constraint);
		_safe_delete(constraint);
	}

	item_a = NULL;
	item_b = NULL;

	type = PhysicConstraintDesc::TypeNone;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
BulletConstraint::BulletConstraint(btDiscreteDynamicsWorld *_world)
{
	type = PhysicConstraintDesc::TypeNone;

	world = _world;
	constraint = NULL;
}
BulletConstraint::~BulletConstraint()
{
	DeleteConstraint();
}
//------------------------------------------------------------------------------