#include "hinge_joint.hpp"
#include "rigid_body.hpp"
#include "constraint_body_body_point.hpp"
#include "constraint_body_body_max_distance.hpp"
#include "physics.hpp"

using namespace std;

//==============================================================
// Hinge_joint
//==============================================================
Hinge_joint::Hinge_joint()
{
  TRACE_METHOD_ONLY(ONCE_2);
}



void Hinge_joint::initialise(Physics * physics,
                             Rigid_body * body0, Rigid_body * body1, 
                             const Vector3 & hinge_axis, 
                             const Vector3 & hinge_pos_abs,
                             const Matrix3 & invMtx0,
                             const Matrix3 & invMtx1,
                             Scalar hinge_half_width,
                             Scalar sideways_slack,
                             Scalar hinge_fwd_angle,
                             Scalar hinge_bck_angle,
                             Scalar damping)
{
  TRACE_METHOD_ONLY(ONCE_2);
  
  m_body0 = body0;
  m_body1 = body1;
  m_hinge_axis = hinge_axis;
  Position hinge_pos_rel0 = (hinge_pos_abs - body0->get_position());
  Position hinge_pos_rel1 = (hinge_pos_abs - body1->get_position());
  m_damping = damping;
  m_using_limit = false;
  
  m_hinge_axis.normalise();
/*  
  Position hinge_pos_rel1 = body0->get_position() + 
    hinge_pos_rel0 - body1->get_position();
*/
  // generate the two positions relative to each body
  Position rel_pos0a = hinge_pos_rel0 + hinge_half_width * hinge_axis;
  Position rel_pos0b = hinge_pos_rel0 - hinge_half_width * hinge_axis;

  Position rel_pos1a = hinge_pos_rel1 + hinge_half_width * hinge_axis;
  Position rel_pos1b = hinge_pos_rel1 - hinge_half_width * hinge_axis;

  const Scalar hinge_allowed_distance = 0.005f;
  const Scalar hinge_allowed_distance1 = hinge_half_width * sideways_slack;
  const Scalar hinge_timescale = -3.0f;

  m_point_constraint.initialise(body0, invMtx0 * hinge_pos_rel0, 
                                body1, invMtx1 * hinge_pos_rel1,
                                hinge_allowed_distance, hinge_timescale);
  physics->add_constraint(&m_point_constraint);

  m_side_point_constraints[0].initialise(body0, invMtx0 * rel_pos0a, 
                                         body1, invMtx1 * rel_pos1a,
                                         hinge_allowed_distance1);
  physics->add_constraint(&m_side_point_constraints[0]);

  m_side_point_constraints[1].initialise(body0, invMtx0 * rel_pos0b, 
                                         body1, invMtx1 * rel_pos1b,
                                         hinge_allowed_distance1);
  physics->add_constraint(&m_side_point_constraints[1]);

  m_hinge_fwd_angle = hinge_fwd_angle;
  if (hinge_fwd_angle <= MAX_HINGE_ANGLE_LIMIT)
  {
    // choose a direction that is perpendicular to the hinge
    Vector3 perp_dir(0.0f, 0.0f, 1.0f);
    if (dot(perp_dir, hinge_axis) > 0.1f)
      perp_dir = Vector3(0.0f, 1.0f, 0.0f);
    // now make it perpendicular to the hinge
    Vector3 side_axis = cross(hinge_axis, perp_dir);
    perp_dir = cross(side_axis, hinge_axis).normalise();

    // the length of the "arm"
    // TODO take this as a parameter? what's the effect of changing it?
    Scalar len = 10.0f * hinge_half_width;

    // Choose a position using that dir. this will be the anchor point for
    // body 0. relative to hinge
    Position hinge_rel_anchor_pos0 = perp_dir * len;

    // anchor point for body 1 is chosen to be in the middle of the angle range.
    // relative to hinge
    Scalar angle_to_middle = 0.5f * (hinge_fwd_angle - hinge_bck_angle);
    Position hinge_rel_anchor_pos1 = rotation_matrix(-angle_to_middle, hinge_axis) * 
      hinge_rel_anchor_pos0;

    // work out the "string" length
    Scalar hinge_half_angle = 0.5f * (hinge_fwd_angle + hinge_bck_angle);
    Scalar allowed_distance = len * 2 * sin_deg(hinge_half_angle * 0.5f);

    Position hinge_pos = body0->get_position() + hinge_pos_rel0;
    Position rel_pos0c = hinge_pos + hinge_rel_anchor_pos0 - body0->get_position();
    Position rel_pos1c = hinge_pos + hinge_rel_anchor_pos1 - body1->get_position();

    m_max_distance_constraint.initialise(body0, invMtx0 * rel_pos0c, 
                                         body1, invMtx1 * rel_pos1c,
                                         allowed_distance);
    physics->add_constraint(&m_max_distance_constraint);
    m_using_limit = true;
  }

  physics->add_controller(this);
}

void Hinge_joint::deinit(Physics * physics) {
  physics->remove_constraint(&m_point_constraint);
  physics->remove_constraint(&m_side_point_constraints[0]);
  physics->remove_constraint(&m_side_point_constraints[1]);

  if (m_hinge_fwd_angle <= MAX_HINGE_ANGLE_LIMIT)
  {
    physics->remove_constraint(&m_max_distance_constraint);
  }

  physics->remove_controller(this);
}


//==============================================================
// update
//==============================================================
void Hinge_joint::update(Scalar dt)
{
  // apply damping due to relative rotation around the hinge axis
  // the hinge axis was specified in the world frame...

  Vector3 hinge_axis = m_body0->get_orientation() * m_hinge_axis;

  Scalar mom0 = dot(m_body0->get_world_inertia() * 
                    m_body0->get_rotation(), hinge_axis);
  Scalar mom1 = dot(m_body1->get_world_inertia() * 
                    m_body1->get_rotation(), hinge_axis);

  Vector3 torque0 = ((mom1 - mom0) * m_damping) * hinge_axis;

  m_body0->add_world_torque(torque0);
  m_body1->add_world_torque(-torque0);
}
