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

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

#include "physics/particle_contact.hpp"

#include "demo/bridge.hpp"

#include "assert.h"

namespace demo {
  void bridge::cable_interpolator::interp_add(float n)
  {
    const int points = particles_length / 2;

    interp += n;
    if (interp < 0) {
      cable_ix -= 1;
      assert(n < 0 && n >= -1);
      interp = 1.0 + n;
    }
    if (interp > 1) {
      cable_ix += 1;
      assert(n > 0 && n <= 1);
      interp = n;
    }

    if (cable_ix >= (points - 1)) {
      interp = 1.0;
      cable_ix = points - 2;
    }

    if (cable_ix < 0) {
      interp = 0;
      cable_ix = 0;
    }
  }

  mat4x4 bridge::init()
  {
    interpolator.interp = 0.0;
    interpolator.cable_ix = 0;

    // particles
    for (int i = 0; i < particles_length; i++) {
      float x = float(i / 2) * 2.0f - 5.0f;
      float z = float(i % 2) * 2.0f - 1.0f;

      particles[i].position = vec3(x, 4, z);
      particles[i].velocity = vec3(0, 0, 0);
      particles[i].damping = 0.95f;
      particles[i].acceleration = vec3(0, -9.81, 0);
      //particles[i].acceleration = vec3(0, -0.5, 0);
      particles[i].force_accum = vec3(0, 0, 0);
    }

    // cables
    for (int i = 0; i < cables_length; i++) {
      cables[i].particle[0] = &particles[i];
      cables[i].particle[1] = &particles[i+2];
      cables[i].max_length = 1.9f;
      cables[i].restitution = 0.5f;
    }

    // anchors
    for (int i = 0; i < anchors_length; i++) {
      anchors[i].particle = &particles[i];
      float x = float(i / 2) * 2.2f - 5.5f;
      float z = float(i % 2) * 1.6f - 0.8f;
      anchors[i].anchor = vec3(x, 6, z);
      if (i < 6)
        anchors[i].max_length = float(i / 2) * 0.5f + 3.0f;
      else
        anchors[i].max_length = 5.5f - float(i / 2) * 0.5f;
      anchors[i].restitution = 0.5f;
    }

    // rods
    for (int i = 0; i < rods_length; i++) {
      rods[i].particle[0] = &particles[i * 2];
      rods[i].particle[1] = &particles[i * 2 + 1];
      rods[i].length = 2;
    }

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

    return view_trans;
  }

  static int en_cables = 1;

  void bridge::a()
  //void bridge::update()
  {
    en_cables = !en_cables;
    printf("en cables %d\n", en_cables);
  }

  void bridge::analog(float dl, float dr, float dx, float dy) {
    interpolator.interp_add(dx * 0.5f);
  }

  void bridge::_update()
  {
    for (int i = 0; i < particles_length; i++) {
      particles[i].force_accum = vec3(0, 0, 0);

      if ((i / 2) == interpolator.cable_ix) {
        float mul = 1.0 - interpolator.interp;
        assert(mul >= 0.0 && mul <= 1.0);
        particles[i].inverse_mass = 1.0f / (1.0 + 10.0 * mul);
      } else if ((i / 2) == (interpolator.cable_ix + 1)) {
        float mul = interpolator.interp;
        assert(mul >= 0.0 && mul <= 1.0);
        particles[i].inverse_mass = 1.0f / (1.0 + 10.0 * mul);
      } else {
        particles[i].inverse_mass = 1.0f / 1.0f;
      }
    }

    float duration = 1.f / (60.0f * 3);

    for (int i = 0; i < particles_length; i++) {
      particles[i].integrate(duration);
    }

    // contact generators
    const int max_contacts = 128;
    static physics::particle_contact contacts[max_contacts];
    int contact_ix = 0;

    for (int i = 0; i < anchors_length; i++) {
      int len = anchors[i].fill_contact(&contacts[contact_ix], 1);
      contact_ix += len;
      assert(contact_ix <= 128);
    }

    if (en_cables) {
      for (int i = 0; i < cables_length; i++) {
        int len = cables[i].fill_contact(&contacts[contact_ix], 1);
        contact_ix += len;
        assert(contact_ix <= 128);
      }

      for (int i = 0; i < rods_length; i++) {
        int len = rods[i].fill_contact(&contacts[contact_ix], 1);
        contact_ix += len;
        assert(contact_ix <= 128);
      }
    }

    static physics::particle_contact_resolver resolver;
    resolver.max_iterations = contact_ix * 2;
    resolver.resolve_contacts(contacts, contact_ix, duration);
  }

  void bridge::update()
  //void bridge::a()
  {
    _update();
    _update();
    _update();
    _update();
    _update();
  }

  void bridge::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: bridge", 0xffffffff);
  }

  static inline vec3 lerp(const vec3& a, const vec3& b, float f)
  {
    float dx = b.x - a.x;
    float dy = b.y - a.y;
    float dz = b.z - a.z;

    return {
      a.x + dx * f,
      a.y + dy * f,
      a.z + dz * f,
    };
  }

  void bridge::draw_weight(ta_parameter_writer& writer, const mat4x4& trans)
  {
    vec3 p1 = particles[(interpolator.cable_ix * 2)].position;
    vec3 p2 = particles[((interpolator.cable_ix + 1) * 2)].position;

    vec3 p = lerp(p1, p2, interpolator.interp);
    p.z = 0.0;

    const vec3 color(1.0, 0.5, 0.0);
    draw_cube(writer, trans * translate(p) * scale(0.3f), color);
  }

  void bridge::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 * scale(2.f));

    draw_weight(writer, trans);

    // draw particles
    for (int i = 0; i < particles_length; i++) {
      const vec3 color(1.0, 1.0, 1.0);
      draw_cube(writer, trans * translate(particles[i].position) * scale(0.1f), color);
    }

    // draw cables
    global_polygon_intensity(writer, vec3(1.0, 1.0, 0.0));
    for (int i = 0; i < cables_length; i++) {
      vec3 p1 = screen_transform(trans * cables[i].particle[0]->position);
      vec3 p2 = screen_transform(trans * cables[i].particle[1]->position);

      draw_line(writer,
                p1,
                p2);
    }

    // draw anchors
    global_polygon_intensity(writer, vec3(0.0, 1.0, 1.0));
    for (int i = 0; i < anchors_length; i++) {
      vec3 p1 = screen_transform(trans * anchors[i].particle->position);
      vec3 p2 = screen_transform(trans * anchors[i].anchor);

      draw_line(writer,
                p1,
                p2);
    }

    // draw rods
    global_polygon_intensity(writer, vec3(1.0, 0.0, 1.0));
    for (int i = 0; i < anchors_length; i++) {
      vec3 p1 = screen_transform(trans * rods[i].particle[0]->position);
      vec3 p2 = screen_transform(trans * rods[i].particle[1]->position);

      draw_line(writer,
                p1,
                p2);
    }

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