#include "constraint_body_body_point.hpp"
#include "rigid_body.hpp"

static const Scalar max_vel_mag = 30.0f;

//==============================================================
// Constraint_body_body_point
//==============================================================
Constraint_body_body_point::Constraint_body_body_point(
  Rigid_body * body0, Position body0_pos,
  Rigid_body * body1, Position body1_pos,
  Scalar allowed_distance,
  Scalar timescale)
:
m_body0_pos(body0_pos),
m_body0(body0),
m_body1_pos(body1_pos),
m_body1(body1),
m_allowed_distance(allowed_distance),
m_timescale(timescale)
{
  TRACE_FILE_IF(ONCE_1)
    TRACE("Creating body-body point constraint\n");
}

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

//==============================================================
// initialise
//==============================================================
void Constraint_body_body_point::initialise(Rigid_body * body0, Position body0_pos,
                                            Rigid_body * body1, Position body1_pos,
                                            Scalar allowed_distance,
                                            Scalar timescale)
{
  m_body0_pos = body0_pos;
  m_body1_pos = body1_pos;
  m_body0 = body0;
  m_body1 = body1;
  m_allowed_distance = allowed_distance;
  m_timescale = timescale;
}

//===============================================================================
// pre_apply
//===============================================================================
void Constraint_body_body_point::pre_apply(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  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);

  // add a "correction" based on the deviation of point 0
  const Position deviation = world_pos0 - world_pos1;
  Scalar deviation_amount = deviation.mag();
  Scalar timescale = m_timescale > 0 ? m_timescale : -m_timescale * dt;
  if (timescale < 0.00001f) timescale = dt;

  if (deviation_amount > m_allowed_distance)
  {
    const Vector3 deviation_dir = deviation / deviation_amount;
	  m_vr_extra = ((deviation_amount - m_allowed_distance) / timescale) * deviation_dir;
  }
  else
  {
	  m_vr_extra.set_to(0.0f);
  }
}

//==============================================================
// apply
//==============================================================
bool Constraint_body_body_point::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);

  // add a "correction" based on the deviation of point 0
  Velocity Vr = m_vr_extra + current_vel0 - current_vel1;

  const Scalar normal_vel_sqr = Vr.mag2();
  if (normal_vel_sqr <= 0.0001f)
    return false;
  Scalar normal_vel = sqrt(normal_vel_sqr);

  // limit things
  if (normal_vel > max_vel_mag)
  {
    Vr *= max_vel_mag / normal_vel;
    normal_vel = max_vel_mag;
  }

  const Vector3 N = Vr / normal_vel;

  const Scalar numerator = -normal_vel;
  const 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;

  const Vector3 normal_impulse = (numerator / denominator) * N;

  // Rather than getting the body to apply the impulses, calculate the
  // linear and angular components ourselves to avoid recalculating
  // pos - m_position

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

  m_body0->apply_world_impulse(normal_impulse);
  m_body1->apply_world_impulse(-normal_impulse);
  m_body0->apply_world_ang_impulse(cross(m_r0, normal_impulse));
  m_body1->apply_world_ang_impulse(cross(m_r1, -normal_impulse));
  // 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);
      m_body0->apply_world_ang_impulse(cross(m_r0, -normal_impulse));
    }
  }
  else if (body1_frozen_pre)
  {
    if (!m_body1->should_be_active())
    {
      // OK - reverse the impulse on body1
      m_body1->apply_world_impulse(normal_impulse);
      m_body1->apply_world_ang_impulse(cross(m_r1, normal_impulse));
    }
  }

  return true;
}
