/* 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)
{
	Iterator iter (*this);
	for (iter.Begin(); iter; ) {
		dgTreeNode* const node = iter.GetNode();
		iter ++;
		dgBody* const 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)
{
	Iterator iter (*this);
	for (iter.Begin(); iter; ) {
		dgTreeNode* const node = iter.GetNode();
		iter ++;
		dgConstraint* const joint = node->GetInfo();
		if (joint) {
			Remove (node);
			world.DestroyConstraint (joint);
		}
	}
}


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


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


Newton::Newton (dgFloat32 scale, dgMemoryAllocator* const allocator)
	:dgWorld(allocator) 
	,NewtonDeadBodies(allocator)
	,NewtonDeadJoints(allocator)
	,m_maxTimeStep(DG_TIMESTEP)
	,m_destructor(NULL)
{
}

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

void Newton::UpdatePhysics (dgFloat32 timestep)
{
	Update (timestep);

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

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

void Newton::UpdatePhysicsAsync (dgFloat32 timestep)
{
	UpdateAsync (timestep);

	if (!IsBusy()) {		
		NewtonDeadBodies& bodyList = *this;
		NewtonDeadJoints& jointList = *this;

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


void Newton::DestroyJoint(dgConstraint* const joint)
{
	if (IsBusy()) {		
		// the engine is busy in the previous update, deferred the deletion
		NewtonDeadJoints& jointList = *this;
		jointList.Insert (joint, joint);
	} else {
		dgWorld::DestroyConstraint (joint);
	}
}

void Newton::DestroyBody(dgBody* const body)
{
	if (IsBusy()) {		
		// the engine is busy in the previous update, deferred the deletion
		NewtonDeadBodies& bodyList = *this;
		bodyList.Insert (body, body);
	} else {
		dgWorld::DestroyBody(body);
	}
}


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

	dgAssert (world);
	m_forceArray = m_jointForce;
	if (m_maxDOF > DG_BILATERAL_CONTRAINT_DOF) {
		m_forceArray = (dgForceImpactPair*) world->GetAllocator()->Malloc (dgInt32 (m_maxDOF * sizeof (dgForceImpactPair)));
	}
	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 ++;
	dgAssert (m_rows <= dgInt32 (m_maxDOF));
}

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

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

	SetJacobianDerivative (m_rows, *m_param, jacobian0, jacobian1, &m_forceArray[m_rows]);
	m_rows ++;
	dgAssert (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 damperD)
{
	dgInt32 index; 
	index = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		dgFloat32 accel = CalculateSpringDamperAcceleration (index, *m_param, m_lastJointAngle, m_lastPosit0, m_lastPosit1, springK, damperD);
		SetMotorAcceleration (index, accel, *m_param);
	}
}



void NewtonUserJoint::SetHighFriction (dgFloat32 friction)
{
	dgInt32 index = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		m_param->m_forceBounds[index].m_upper = dgClamp (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 = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		m_param->m_forceBounds[index].m_low = dgClamp (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 = m_rows - 1;
	if ((index >= 0) &&  (index < dgInt32 (m_maxDOF))) {
		stiffness = dgClamp (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 = 0.0f;
	if ((row >= 0) && (row < dgInt32 (m_maxDOF))) {
		force = m_forceArray[row].m_force; 
	}
	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);
}

void NewtonUserJoint::SetMaxContactsForExactSolver (bool mode, dgInt32 MaxCount)
{
	dgUserConstraint::SetMaxContactsForExactSolver (mode, MaxCount);
}
