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

#include "platform/graphics_primitive.hpp"
#include "platform/font.hpp"

#include "demo/collision.hpp"

#include "assert.h"

namespace demo {
  static inline mat3x3 inertia_tensor_coeffs(float ix, float iy, float iz,
                                             float ixy = 0, float ixz = 0, float iyz = 0)
  {
    return mat3x3(  ix, -ixy, -ixz,
                  -ixy,   iy, -iyz,
                  -ixz, -iyz,   iz);
  }

  mat4x4 collision::init()
  {
    plane.body = &plane_body;
    plane.body->position = {0, 0, 0};
    plane.body->orientation = {0, 0, 0, 1};
    plane.body->calculate_derived_data();
    plane.normal = {0, 1, 0};
    plane.distance = 0;
    plane.offset = mat4x4();
    plane.update_transform();

    sphere.body = &sphere_body;
    sphere.body->inverse_mass = 1.0f / 1.0f;
    sphere.body->position = {0, 1, 0};
    sphere.body->orientation = {0, 0, 0, 1};
    sphere.body->velocity = {0, 0, 0};
    sphere.body->rotation = {0, 0, 0};
    sphere.body->set_inertia_tensor(inertia_tensor_coeffs(5.0, 5.0, 5.0));
    sphere.body->linear_damping = 0.998f;
    sphere.body->angular_damping = 0.998f;
    sphere.body->acceleration = {0, -9.81, 0};
    sphere.body->calculate_derived_data();
    sphere.radius = 0.25f;
    sphere.offset = mat4x4();
    sphere.update_transform();

    collision_data.ix = 0;
    collision_data.length = 128;
    collision_data.restitution = 0.2;
    collision_data.friction = 0.9;

    mat4x4 view_trans
      = translate(vec3(0.0, 0.0, 0.5))
      * rotate_x(pi / 4)
      * rotate_y(pi / 4)
      * scale(vec3(-3, -3, 3));

    return view_trans;
  }

  void collision::update()
  {
    sphere.body->clear_accumulators();

    // generate contacts
    collision_data.ix = 0;
    int res = physics::collision_detector::sphere_and_halfspace(sphere, plane, collision_data);
    printf("res %d\n", res);

    // set iterations

    // resolve contacts

    float duration = 1.0f / 600.0f;
    sphere.body->integrate(duration);
    sphere.update_transform();
  }

  void collision::draw_hud(ta_parameter_writer& writer)
  {
    const int title_length = 15;
    const int title_width_2 = (font::ter_u12n.hori_advance * title_length) >> 1;
    const int framebuffer_width_2 = framebuffer::framebuffer.px_width >> 1;
    const int x = framebuffer_width_2 - title_width_2;
    vec3 center_p = vec3(x, 5, 10);

    font::ter_u12n.global(writer);
    font::ter_u12n.draw_string(writer, center_p, "demo: collision", 0xffffffff);
  }

  void collision::draw(ta_parameter_writer& writer, const mat4x4& trans)
  {
    // punch through
    draw_hud(writer);

    writer.append<ta_global_parameter::end_of_list>() =
      ta_global_parameter::end_of_list(para_control::para_type::end_of_list);

    // opaque
    draw_axis(writer, trans);

    draw_icosphere(writer,
                   trans * sphere.transform * scale(sphere.radius),
                   vec3(1, 0, 1));

    vec3 n = plane.normal;
    vec3 k = plane.normal * plane.distance;
    vec3 color(1, 1, 0);
    draw_halfspace(writer,
                   k,
                   n,
                   trans,
                   color);

    writer.append<ta_global_parameter::end_of_list>() =
      ta_global_parameter::end_of_list(para_control::para_type::end_of_list);

  }
}


extern "C"
void memset()
{
}
