#include "CPhysicsRagdoll.h"

IPhysicsRagdoll::IPhysicsRagdoll()
{
}

IPhysicsRagdoll::~IPhysicsRagdoll()
{
	NewtonDestroyRagDoll(m_world, m_ragdoll);
}

void IPhysicsRagdoll::init(NewtonWorld* world, IrrlichtDevice* dev, SPhysicsRagdoll* doll)
{
	m_world = world;
	m_device = dev;

	m_ragdoll = NewtonCreateRagDoll (m_world);

	NewtonRagDollBegin (m_ragdoll);

	NewtonRagDollSetForceAndTorqueCallback (m_ragdoll, applyRagDollForceAndTorqueEvent);

	NewtonRagDollSetTransformCallback (m_ragdoll, setRagDollTransform);

	buildRagDoll(m_ragdoll, NULL, doll->body.lowerChest, 0);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 0), doll->body.upperChest, 1);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 1), doll->body.head, 2);

	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 1), doll->body.rUpperArm, 3);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 3), doll->body.rLowerArm, 4);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 1), doll->body.lUpperArm, 5);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 5), doll->body.lLowerArm, 6);
	
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 0), doll->body.rUpperLeg, 7);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 7), doll->body.rLowerLeg, 8);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 0), doll->body.lUpperLeg, 9);
	buildRagDoll(m_ragdoll, NewtonRagDollFindBone(m_ragdoll, 9), doll->body.lLowerLeg, 10);
	
	NewtonRagDollEnd (m_ragdoll);
}

void IPhysicsRagdoll::buildRagDoll(NewtonRagDoll* ragDoll, NewtonRagDollBone* parentDollBone, ISceneNode* bone, int id)
{
	NewtonBody *body;
	
	/// Get the box of the bone.
	aabbox3d<f32> box (bone->getBoundingBox());
	vector3df size (box.getExtent () * IrrToNewton); 

	/// Get the bone local matrix and comver to newton units.
	matrix4 matrix (bone->getRelativeTransformation());
	matrix.setTranslation (matrix.getTranslation() * IrrToNewton);

	NewtonCollision* collision = NewtonCreateBox(m_world, size.X, size.Y, size.Z, NULL);

	/// Create a rag doll bone at the origin of this bone.
	NewtonRagDollBone* doll_bone = NewtonRagDollAddBone (ragDoll, parentDollBone, bone, 10.0f, matrix.pointer(), collision, &size.X); // Irrlicht 1.3
	NewtonRagDollBoneSetID (doll_bone, id);

	NewtonReleaseCollision(m_world,collision);
	
	/// Set the rad doll bone limits.
	/// The x axis is usually the box main axis
	vector3df coneDir (1.0f, 0.0f, 0.0f);
	// The z axis is usually the box plane 
	vector3df lateralDir (0.0f, 0.0f, 1.0f);

	// the axis limits are specifiied in global space, rotate by the joint matrix
	matrix4 boneMatrix (bone->getAbsoluteTransformation());
	boneMatrix.rotateVect (coneDir);
	boneMatrix.rotateVect (lateralDir);

	// set the limis
	NewtonRagDollBoneSetLimits (doll_bone, &coneDir.X, -PI * 0.25f, PI * 0.25f, -PI * 0.5f, &lateralDir.X, 0.0f, 0.0f);

	// get the bone rigid body and freeze it
	body = NewtonRagDollBoneGetBody (doll_bone);	
	NewtonWorldFreezeBody (m_world, body);

	// recusivelly add old bones to the rag doll
/*	const list<ISceneNode*>& children = bone->getChildren();
	for (list<ISceneNode*>::Iterator it = children.begin(); it != children.end(); ++it) 
	{
		child = *it;
		buildRagDoll (ragdoll, doll_bone, child, vector3df(0,0,0));
		cout << *it<< endl;
	}*/
}

void IPhysicsRagdoll::setRagDollTransform (const NewtonRagDollBone* bone)
{
	matrix4 matrix;
	ISceneNode* model;

	// get the model from the rag doll bone 
	model = (ISceneNode*) NewtonRagDollBoneGetUserData (bone);

	// get the local matrix for this bone
	NewtonRagDollBoneGetLocalMatrix (bone, matrix.pointer()); // Irrlicht 1.3

	// Position the node
	model->setPosition(matrix.getTranslation() * NewtonToIrr);// set position
	
	vector3df euler;
	// set the angle for the node
	NewtonGetEulerAngle (matrix.pointer(), &euler.X);
	model->setRotation(euler * InvPi);
}

void IPhysicsRagdoll::applyRagDollForceAndTorqueEvent (const NewtonBody* body) 
{ 
   float mass; 
   float Ixx; 
   float Iyy; 
   float Izz; 

   NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz); 

   vector3df force (0.0f, -9.8f * mass, 0.0f); 
   vector3df torque (0.0f, 0.0f, 0.0f);  
   
   NewtonBodyAddForce (body, &force.X); 
   NewtonBodyAddTorque (body, &torque.X); 
}