#include "math/float_types.hpp"
#include "math/transform.hpp"

#include "body.hpp"

namespace physics {

  void rigid_body::calculate_derived_data()
  {
    orientation = normalize(orientation);

    transform_matrix = translate(position) * rotate_quaternion(orientation);

    mat3x3 t = submatrix(transform_matrix, 3, 3);

    inverse_inertia_tensor_world = t * inverse_inertia_tensor * transpose(t);
  }

  void rigid_body::set_inertia_tensor(const mat3x3& inertia_tensor)
  {
    inverse_inertia_tensor = inverse(inertia_tensor);
  }

  void rigid_body::clear_accumulators()
  {
    force_accum = vec3(0, 0, 0);
    torque_accum = vec3(0, 0, 0);
  }

  void rigid_body::add_force_at_local_point(const vec3& force,
                                            const vec3& point)
  {
    vec3 pt = transform_matrix * point;
    add_force_at_point(force, pt);
  }

  void rigid_body::add_force_at_point(const vec3& force,
                                      const vec3& point)
  {
    // relative to center of mass
    vec3 pt = point - position;

    force_accum += force;
    torque_accum += cross(pt, force);
  }

  void rigid_body::integrate(float duration)
  {
    last_frame_acceleration = acceleration;
    last_frame_acceleration += force_accum * inverse_mass;

    vec3 angular_acceleration = inverse_inertia_tensor_world * torque_accum;

    velocity += last_frame_acceleration * duration;
    rotation += angular_acceleration * duration;

    // drag
    velocity *= linear_damping;
    rotation *= angular_damping;

    position += velocity * duration;

    orientation += rotation * duration;

    calculate_derived_data();

    clear_accumulators();
  }
}
