#include "constraint_fixed_point.hpp"
#include "rigid_body.hpp"

Constraint_fixed_point::Constraint_fixed_point(Rigid_body * body,
                                               Position point_on_body,
                                               Position world_position)
:
m_body(body),
m_point_on_body(point_on_body),
m_world_position(world_position)
{
  TRACE("Creating fixed world point constraint\n");
}

bool Constraint_fixed_point::apply(Scalar dt)
{
  TRACE_METHOD_ONLY(MULTI_FRAME_1);
  const Position world_pos = m_body->get_position() + m_body->get_orientation() * m_point_on_body;
  const Position R = world_pos - m_body->get_position();
  const Velocity current_vel = m_body->get_velocity() + cross(m_body->get_rotation(), R);

  // add an extra term to get us back to the original position
  Velocity desired_vel;

 	const Scalar allowed_deviation = 0.01f;
	const Scalar timescale = 4 * dt;
  Position deviation = world_pos - m_world_position;
  Scalar deviation_distance = deviation.mag();
  if (deviation_distance > allowed_deviation)
  {
    Vector3 deviation_dir = deviation / deviation_distance;
    desired_vel = (-(deviation_distance - allowed_deviation) / timescale) * deviation_dir;
  }
  else
  {
    desired_vel.set_to(0.0f);
  }

  // need an impulse to take us from the current vel to the desired vel
  Vector3 N = current_vel - desired_vel;
  Scalar normal_vel = N.mag();
  if (normal_vel < 0.01f)
    return false;

  N /= normal_vel;

  Scalar numerator = -normal_vel;
  Scalar denominator = m_body->get_inv_mass() + dot(N, cross(m_body->get_world_inv_inertia() * (cross(R, N)), R));

  if (denominator < 0.0001f)
    return false;

  Scalar normal_impulse = numerator / denominator;

  m_body->apply_world_impulse(normal_impulse * N, world_pos);

  return true;
}

