/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
* 
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* 
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "NewtonStdAfx.h"
#include "NewtonClass.h"





NewtonDeadBodies::NewtonDeadBodies(dgMemoryAllocator* const allocator)
	:dgTree<dgBody*, void*>(allocator)
{
	Insert((dgBody*)NULL, 0);
}

void NewtonDeadBodies::DestroyBodies(Newton& world)
{
	dgBody* body;
	dgTreeNode* node;

	Iterator iter (*this);
	for (iter.Begin(); iter; ) {
		node = iter.GetNode();
		iter ++;
		body = node->GetInfo();
		if (body) {
			Remove (node);
			world.DestroyBody(body);
		}
	}
}


NewtonDeadJoints::NewtonDeadJoints(dgMemoryAllocator* const allocator)
	:dgTree<dgConstraint*, void*>(allocator)
{
	Insert((dgConstraint*)NULL, 0);
}


void NewtonDeadJoints::DestroyJoints(Newton& world)
{
	dgTreeNode* node;
	dgConstraint* joint;

	Iterator iter (*this);
	for (iter.Begin(); iter; ) {
		node = iter.GetNode();
		iter ++;
		joint = node->GetInfo();
		if (joint) {
			Remove (node);
			world.DestroyConstraint (joint);
		}
	}
}


void* Newton::DefaultAllocMemory (dgInt32 size)
{
	return malloc (size_t (size));
}


void Newton::DefaultFreeMemory (void *ptr, dgInt32 size)
{
	free (ptr);
}


Newton::Newton (dgFloat32 scale, dgMemoryAllocator* const allocator)
	:dgWorld(allocator), 
	 NewtonDeadBodies(allocator),
	 NewtonDeadJoints(allocator)
{
	m_updating = false;
	g_maxTimeStep = dgFloat32 (1.0f/ 60.0f);

	m_destructor = NULL;
//	SetGlobalScale (scale);
}

Newton::~Newton ()
{
	if (m_destructor) {
		m_destructor ((NewtonWorld*)this);
	}
}

void Newton::UpdatePhysics (dgFloat32 timestep)
{
	m_updating = true;
	Update (timestep);

//	RagdollHeaderActiveList::UpdateMatrix();
	m_updating = false;

	NewtonDeadBodies& bodyList = *this;
	NewtonDeadJoints& jointList = *this;

	jointList.DestroyJoints (*this);
	bodyList.DestroyBodies (*this);
}

void Newton::DestroyJoint(dgConstraint* joint)
{
	if (m_updating) {
		NewtonDeadJoints& jointList = *this;
		jointList.Insert (joint, joint);
	} else {
		dgWorld::DestroyConstraint (joint);
	}
}

void Newton::DestroyBody(dgBody* body)
{
	if (m_updating) {
		NewtonDeadBodies& bodyList = *this;
		bodyList.Insert (body, body);
	} else {
		dgWorld::DestroyBody(body);
	}
}


NewtonUserJoint::NewtonUserJoint (
	dgWorld* world, 
	dgInt32 maxDof, 
	NewtonUserBilateralCallBack callback, 
	NewtonUserBilateralGetInfoCallBack getInfo,
	dgBody *dyn0, 
	dgBody *dyn1)
	:dgUserConstraint (world, dyn0, dyn1, 1)
{
	m_rows = 0;
	m_maxDOF = dgUnsigned8(maxDof);
	m_jacobianFnt = callback;
	m_getInfoCallback = getInfo;

	_ASSERTE (world);
	m_forceArray = m_jointForce;
	if (m_maxDOF > 24) {
		_ASSERTE (0);
		m_forceArray = (dgFloat32*) world->GetAllocator()->Malloc (dgInt32 (m_maxDOF * sizeof (dgFloat32)));
	}
	memset (m_forceArray, 0, m_maxDOF * sizeof (dgFloat32));
}

NewtonUserJoint::~NewtonUserJoint ()
{
	if (m_forceArray != m_jointForce) {
		m_body0->GetWorld()->GetAllocator()->Free (m_forceArray);
	}

}


dgUnsigned32 NewtonUserJoint::JacobianDerivative (dgContraintDescritor& params)
{
	m_rows = 0;
	m_param = &params;
	m_jacobianFnt ((NewtonJoint*)this, params.m_timestep, params.m_threadIndex);
	return dgUnsigned32 (m_rows);
}


void NewtonUserJoint::AddLinearRowJacobian (const dgVector& pivot0, const dgVector& pivot1, const dgVector& dir)
{
	dgPointParam pointData;
    InitPointParam (pointData, m_stiffness, pivot0, pivot1);

	m_lastPosit0 = pivot0;
	m_lastPosit1 = pivot1;
	m_lastJointAngle = dgFloat32 (0.0f);
	CalculatePointDerivative (m_rows, *m_param, dir, pointData, &m_forceArray[m_rows]); 
	m_rows ++;
	_ASSERTE (m_rows <= dgInt32 (m_maxDOF));
}

void NewtonUserJoint::AddAngularRowJacobian (const dgVector& dir, dgFloat32 relAngle)
{
	m_lastPosit0 = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	m_lastPosit1 = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	m_lastJointAngle = relAngle;
	CalculateAngularDerivative (m_rows, *m_param, dir, m_stiffness, relAngle, &m_forceArray[m_rows]); 
	m_rows ++;
	_ASSERTE (m_rows <= dgInt32 (m_maxDOF));
}

void NewtonUserJoint::AddGeneralRowJacobian (const dgFloat32* jacobian0, const dgFloat32* jacobian1)
{
	m_lastPosit0 = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	m_lastPosit1 = dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	m_lastJointAngle = 0.0f;

	SetJacobianDerivative (m_rows, *m_param, jacobian0, jacobian1, &m_forceArray[m_rows]);
	m_rows ++;
	_ASSERTE (m_rows <= dgInt32 (m_maxDOF));
}


void NewtonUserJoint::SetAcceleration (dgFloat32 acceleration)
{
	dgInt32 index; 
	index = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
//		m_param->m_jointAccel[index] = acceleration;
		SetMotorAcceleration (index, acceleration, *m_param);
	}
}

void NewtonUserJoint::SetSpringDamperAcceleration (dFloat springK, dFloat springD)
{
	dgInt32 index; 
	index = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		dgFloat32 accel;
		accel = CalculateSpringDamperAcceleration (index, *m_param, m_lastJointAngle, m_lastPosit0, m_lastPosit1, springK, springD);
		_ASSERTE (0);
//		m_param->m_jointAccel[index] = accel;
		SetMotorAcceleration (index, accel, *m_param);
	}
}



void NewtonUserJoint::SetHighFriction (dgFloat32 friction)
{
	dgInt32 index; 
	index = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		m_param->m_forceBounds[index].m_upper = ClampValue (friction, dgFloat32(0.001f), dgFloat32(DG_MAX_BOUND));
		m_param->m_forceBounds[index].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
	}
}

void NewtonUserJoint::SetLowerFriction (dgFloat32 friction)
{
	dgInt32 index; 
	index = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		m_param->m_forceBounds[index].m_low = ClampValue (friction, dgFloat32(DG_MIN_BOUND), dgFloat32(-0.001f));
		m_param->m_forceBounds[index].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
	}
}


void NewtonUserJoint::SetRowStiffness (dgFloat32 stiffness)
{
	dgInt32 index; 
	index = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		stiffness = ClampValue (stiffness, dgFloat32(0.0f), dgFloat32(1.0f));
		stiffness = 100.0f - stiffness * 99.0f; 
		m_param->m_jointStiffness[index] = stiffness;
	}
}



dgFloat32 NewtonUserJoint::GetRowForce (dgInt32 row) const
{
	dgFloat32 force;

	force = 0.0f;
	if ((row >= 0) && (row < dgInt32 (m_maxDOF))) {
		force = m_forceArray[row]; 
	}
	return force;
}


void NewtonUserJoint::GetInfo (dgConstraintInfo* const info) const
{
	memset (info, 0, sizeof (dgConstraintInfo));
	if (m_getInfoCallback) {
		InitInfo (info);
		m_getInfoCallback ((NewtonJoint*) this, (NewtonJointRecord*)info);
	}
}


void NewtonUserJoint::SetUpdateFeedbackFunction (NewtonUserBilateralCallBack getFeedback)
{
	dgUserConstraint::SetUpdateFeedbackFunction ((ConstraintsForceFeeback) getFeedback);
}