/* 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);
}

