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

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

#include "demo/sailboat.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);
  }

  static inline mat3x3 block_inertia_tensor(const vec3& half_sizes, float mass)
  {
    vec3 squares = half_sizes * half_sizes;
    return inertia_tensor_coeffs(0.3f * mass * (squares.y + squares.z),
                                 0.3f * mass * (squares.x + squares.z),
                                 0.3f * mass * (squares.x + squares.y));
  }

  mat4x4 sailboat::init()
  {
    buoyancy.center_of_buoyancy = vec3(0.0f, 0.5f, 0.0f);
    buoyancy.max_depth = 1.0f;
    buoyancy.volume = 3.0f;
    buoyancy.water_height = 1.6f;
    buoyancy.liquid_density = 1000.0f;

    body.position = vec3(0, 1.6f, 0);
    body.orientation = vec4(0, 0, 0, 1);
    body.velocity = vec3(0, 0, 0);
    body.rotation = vec3(0, 0, 0);
    body.inverse_mass = 1.0f / 200.0f;

    mat3x3 inertia_tensor = block_inertia_tensor(vec3(2, 1, 1), 100.0f);
    body.set_inertia_tensor(inertia_tensor);

    body.linear_damping = 0.9963f;
    body.angular_damping = 0.9963f;
    body.acceleration = vec3(0, -9.81, 0);
    body.calculate_derived_data();

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

    return view_trans;
  }

  void sailboat::update()
  {
    body.clear_accumulators();

    float duration = 1.0f / 60.0f;

    buoyancy.update_force(&body, duration);

    body.integrate(duration);
  }

  void sailboat::draw_hud(ta_parameter_writer& writer)
  {
    const int title_length = 16;
    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: sailboat", 0xffffffff);
  }

  void sailboat::draw_boat(ta_parameter_writer& writer, const mat4x4& trans)
  {
    vec3 color = vec3(1.0, 0.5, 0.0);

    // left hull
    const mat4x4 tl
      = translate(vec3(0, 0, -1.0f))
      * scale(vec3(2.f, 0.4f, 0.4f))
      * scale(0.5f);
    draw_cube(writer, trans * tl, color);

    // right hull
    const mat4x4 tr
      = translate(vec3(0, 0, 1.0f))
      * scale(vec3(2.f, 0.4f, 0.4f))
      * scale(0.5f);
    draw_cube(writer, trans * tr, color);

    // deck
    vec3 deck_color = vec3(1.0, 0.0, 1.0);
    const mat4x4 td
      = translate(vec3(0, 0.3f, 0.0f))
      * scale(vec3(1.0f, 0.1f, 2.0f))
      * scale(0.5f);
    draw_cube(writer, trans * td, deck_color);

    // mast
    vec3 mast_color = vec3(0.0, 1.0, 1.0);
    const mat4x4 tm
      = translate(vec3(0, 1.8f, 0))
      * scale(vec3(0.1f, 3.0f, 0.1f))
      * scale(0.5f);
    draw_cube(writer, trans * tm, mast_color);
  }

  void sailboat::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_boat(writer, trans * body.transform_matrix);

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