#include "constraint_body_body_max_distance.hpp"
#include "rigid_body.hpp"

// maximum relative velocity induced at the constraint points
static const Scalar max_vel_mag = 30.0f;

//==============================================================
// Constraint_body_body_max_distance
//==============================================================
Constraint_body_body_max_distance::Constraint_body_body_max_distance(
  Rigid_body * body0, Position body0_pos,
  Rigid_body * body1, Position body1_pos,
  Scalar max_distance)
:
m_body0(body0),
m_body1(body1),
m_body0_pos(body0_pos),
m_body1_pos(body1_pos),
m_max_distance(max_distance)
{
  TRACE_FILE_IF(ONCE_1)
    TRACE("Creating body-body max distance constraint\n");
}

//==============================================================
// Constraint_body_body_max_distance
//==============================================================
Constraint_body_body_max_distance::Constraint_body_body_max_distance()
  :
  m_body0(0),
  m_body1(0)
{
}

//==============================================================
// initialise
//==============================================================
void Constraint_body_body_max_distance::initialise(
  Rigid_body * body0, Position body0_pos,
  Rigid_body * body1, Position body1_pos,
  Scalar max_distance)
{
  m_body0_pos = body0_pos;
  m_body1_pos = body1_pos;
  m_body0 = body0;
  m_body1 = body1;
  m_max_distance = max_distance;
}

//==============================================================
// pre_apply
//==============================================================
void Constraint_body_body_max_distance::pre_apply(Scalar dt)
{
  m_r0 = m_body0->get_orientation() * m_body0_pos;
  m_r1 = m_body1->get_orientation() * m_body1_pos;

  const Position world_pos0 = m_body0->get_position() + m_r0;
  const Position world_pos1 = m_body1->get_position() + m_r1;
  m_world_pos = 0.5f * (world_pos0 + world_pos1);

  // current location of point 0 relative to point 1
  m_current_rel_pos0 = world_pos0 - world_pos1;
}

//==============================================================
// apply
//==============================================================
bool Constraint_body_body_max_distance::apply(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  bool body0_frozen_pre = m_body0->get_activity_state() == Rigid_body::FROZEN;
  bool body1_frozen_pre = m_body1->get_activity_state() == Rigid_body::FROZEN;
  if (body0_frozen_pre && body1_frozen_pre)
    return false;

  const Velocity current_vel0 = m_body0->get_velocity() + cross(m_body0->get_rotation(), m_r0);
  const Velocity current_vel1 = m_body1->get_velocity() + cross(m_body1->get_rotation(), m_r1);

  // predict a new location
  Position pred_rel_pos0 = m_current_rel_pos0 + (current_vel0 - current_vel1) * dt;

  // if the new position is out of range then clamp it
  Position clamped_rel_pos0 = pred_rel_pos0;
  Scalar clamped_rel_pos0_mag = clamped_rel_pos0.mag();
  if (clamped_rel_pos0_mag <= 0.0f)
    return false;
  if (clamped_rel_pos0_mag > m_max_distance)
    clamped_rel_pos0 *= m_max_distance / clamped_rel_pos0_mag;

  // now claculate desired vel based on the current pos, new/clamped pos and dt
  Velocity desired_rel_vel0 = (clamped_rel_pos0 - m_current_rel_pos0) / dt;

  // Vr is -ve the total velocity change
  Velocity Vr = (current_vel0 - current_vel1) - desired_rel_vel0;

  Scalar normal_vel = Vr.mag();

  if (normal_vel > max_vel_mag)
  {
    Vr *= (max_vel_mag / normal_vel);
    normal_vel = max_vel_mag;
  }
  else if (normal_vel <= 0.001f)
  {
    return false;
  }

  const Vector3 N = Vr / normal_vel;

  Scalar numerator = -normal_vel;
  Scalar denominator = m_body0->get_inv_mass() + m_body1->get_inv_mass() + 
    dot(N, cross(m_body0->get_world_inv_inertia() * (cross(m_r0, N)), m_r0)) + 
    dot(N, cross(m_body1->get_world_inv_inertia() * (cross(m_r1, N)), m_r1));

  if (denominator < 0.0001f)
    return false;

  Scalar normal_impulse = numerator / denominator;

  m_body0->apply_world_impulse(normal_impulse * N, m_world_pos);
  m_body1->apply_world_impulse(-normal_impulse * N, m_world_pos);

  // if one of the bodies is frozen, then if it is to stay frozen remove the impulse
  // from it. Should really apply it to the other body... but that tends to wake it
  // up (and if one is frozen it's probably quite quiet).
  if (body0_frozen_pre)
  {
    if (!m_body0->should_be_active())
    {
      // OK - reverse the impulse on body0
      m_body0->apply_world_impulse(-normal_impulse * N, m_world_pos);
    }
  }
  else if (body1_frozen_pre)
  {
    if (!m_body1->should_be_active())
    {
      // OK - reverse the impulse on body1
      m_body1->apply_world_impulse(normal_impulse * N, m_world_pos);
    }
  }

  return true;
}
