#include "physics/collide.hpp"

namespace physics {

  int collision_detector::sphere_and_sphere(const collision_sphere& a,
                                            const collision_sphere& b,
                                            collision_data& data)
  {
    if (data.ix >= data.length)
      return 0;

    vec3 a_position = col(a.transform, 3);
    vec3 b_position = col(b.transform, 3);

    vec3 mid = a_position - b_position;
    float length = magnitude(mid);

    if (length >= (a.radius + b.radius)) {
      return 0;
    }

    vec3 normal = mid * (1.0f / length);
    contact * contact = data.add_contact();
    contact->contact_normal = normal;
    contact->contact_point = a_position + mid * 0.5f;
    contact->penetration = a.radius + b.radius - length;
    contact->body[0] = a.body;
    contact->body[1] = b.body;
    contact->restitution = data.restitution;
    contact->friction = data.friction;

    return 1;
  }

  int collision_detector::sphere_and_halfspace(const collision_sphere& sphere,
                                               const collision_plane& plane,
                                               collision_data& data)
  {
    if (data.ix >= data.length)
      return 0;

    vec3 position = col(sphere.transform, 3);
    float distance = dot(plane.normal, position) - sphere.radius - plane.distance;

    if (distance >= 0)
      return 0;

    contact * contact = data.add_contact();
    contact->contact_normal = plane.normal;
    contact->penetration = -distance;
    contact->contact_point = position - plane.normal * (distance + sphere.radius);
    contact->body[0] = sphere.body;
    contact->body[1] = nullptr;
    contact->restitution = data.restitution;
    contact->friction = data.friction;

    return 1;
  }

  int collision_detector::sphere_and_plane(const collision_sphere& sphere,
                                           const collision_plane& plane,
                                           collision_data& data)
  {
    if (data.ix >= data.length)
      return 0;

    vec3 position = col(sphere.transform, 3);

    float distance = dot(plane.normal, position) - plane.distance;

    if (distance * distance > sphere.radius * sphere.radius) {
      return 0;
    }

    vec3 normal;
    float penetration;
    if (distance < 0) {
      normal = -plane.normal;
      penetration = distance;
    } else {
      normal = plane.normal;
      penetration = -distance;
    }
    penetration += sphere.radius;

    contact * contact = data.add_contact();
    contact->contact_normal = normal;
    contact->penetration = penetration;
    contact->contact_point = position - plane.normal * distance;
    contact->body[0] = sphere.body;
    contact->body[1] = nullptr;
    contact->restitution = data.restitution;
    contact->friction = data.friction;

    return 1;
  }

  int collision_detector::box_and_halfspace(const collision_box& box,
                                            const collision_plane& plane,
                                            collision_data& data)
  {
    if (data.ix >= data.length)
      return 0;

    return 0;
  }
}