#include "cylinder.hpp"
#include <vector>

using namespace std;

extern bool show_deactivation;

//==============================================================
// Cylinder
//==============================================================
Cylinder::Cylinder(Physics * physics, 
                   Scalar radius, Scalar length, 
                   int slices, int stacks, Scalar mass)
:
Object(physics),
m_collision_body(this),
m_radius(radius),
m_length(length)
{
  TRACE_FILE_IF(ONCE_1)
    TRACE("Creating cylinder\n");
  set_mass(mass);
  Scalar Ix = (1.0f / 2.0f) * mass * radius * radius;
  Scalar Iy = (1.0f / 12.0f) * mass * length * length + 0.25f * mass * radius * radius;
  Scalar Iz = Iy;
  set_body_inertia(Ix, Iy, Iz);

  int mesh_num = 16;

  if (stacks < 2)
    stacks = 2;
  if (slices < 3)
    slices = 3;

  vector<Position> points;
  vector<Position> edge_points;
  vector<Collision_body_edge> edges;

  int i, j;
  // points
  for (i = 0 ; i < slices ; ++i)
  {
    for (j = 0 ; j < stacks ; ++j)
    {
      Scalar x0 = -0.5f * length + length * (j + 0.0f) / stacks;
      Scalar x1 = -0.5f * length + length * (j + 1.0f) / stacks;

      Scalar theta0 = 360.0f * (i + 0.0f) / slices;

      Scalar y0 = radius * cos_deg(theta0);
      Scalar z0 = radius * sin_deg(theta0);

      // the sides
      points.push_back(Position(x0, y0, z0));
      if (j == (stacks - 1))
        points.push_back(Position(x1, y0, z0));
    }
  }
  // add a point on each end
  points.push_back(Position(0.5f * length, 0, 0));
  points.push_back(Position(-0.5f * length, 0, 0));
  // triangles
  for (i = 0 ; i < slices ; ++i)
  {
    Scalar x0 = -0.5f * length;
    Scalar x1 = 0.5f * length;

    Scalar theta0 = 360.0f * (i + 0.0f) / slices;
    Scalar theta1 = 360.0f * (i + 1.0f) / slices;

    Scalar y0 = radius * cos_deg(theta0);
    Scalar y1 = radius * cos_deg(theta1);
    Scalar z0 = radius * sin_deg(theta0);
    Scalar z1 = radius * sin_deg(theta1);

    // the sides
    m_triangles.push_back(Collision_triangle(Position(x0, y0, z0), Position(x1, y1, z1), Position(x1, y0, z0)));
    m_triangles.push_back(Collision_triangle(Position(x0, y0, z0), Position(x0, y1, z1), Position(x1, y1, z1)));

    // the ends
    m_triangles.push_back(Collision_triangle(Position(x0, 0, 0), Position(x0, y1, z1), Position(x0, y0, z0)));
    m_triangles.push_back(Collision_triangle(Position(x1, 0, 0), Position(x1, y0, z0), Position(x1, y1, z1)));
  }

  m_collision_body.initialise(mesh_num, mesh_num, mesh_num, 
                              1.2f,
                              m_triangles,
                              points,
                              edge_points,
                              edges,
                              this);
  m_colour_left = Vector3(ranged_random(0.2f, 1.0f),
                          ranged_random(0.2f, 1.0f), 
                          ranged_random(0.2f, 1.0f));
  m_colour_right = Vector3(ranged_random(0.2f, 1.0f),
                          ranged_random(0.2f, 1.0f), 
                          ranged_random(0.2f, 1.0f));

}

//==============================================================
// draw_triangles
//==============================================================
void Cylinder::draw_triangles() const
{
}

//==============================================================
// add_external_forces
//==============================================================
void Cylinder::add_external_forces(Scalar dt)
{
  add_gravity();
}

//==============================================================
// get_mesh_info
//==============================================================
bool Cylinder::get_mesh_info(const Position & pos, 
                             bool & inside,
                             Vector3 & vector_to_surface) const
{
  // the +ve end of the cylinder
  Scalar dx = m_length * 0.5f;
  
  Scalar dx1 = dx - m_radius;
  
  Scalar r = hypot(pos[1], pos[2]);

  if ( (r > m_radius) || (pos[0] > dx) || (pos[0] < -dx) )
    inside = false;
  else
    inside = true;

  Position surface_pos;

  if (r == 0.0f)
  {
    // nasty case
    if (m_radius < dx)
    {
      surface_pos = Position(0, m_radius, 0);
    }
    else
    {
      if (pos[0] > 0)
        surface_pos = Position(dx, 0, 0);
      else
        surface_pos = Position(-dx, 0, 0);
    }
  }
  else if ( (pos[0] > dx1 + r) || (pos[0] < -(dx1 + r)) )
  {
    // inside the cone that results in the nearest surface point being on the +ve/-ve end
    surface_pos = pos;
    if (r > m_radius)
    {
      surface_pos[1] *= m_radius / r;
      surface_pos[2] *= m_radius / r;
    }
    if (pos[0] > 0)
      surface_pos[0] = dx;
    else if (pos [0] < 0)
      surface_pos[0] = -dx;
  }
  else
  {
    // nearest point is on the curved cylinder surface.
    surface_pos = pos;
    surface_pos[1] *= m_radius / r;
    surface_pos[2] *= m_radius / r;
    if (pos[0] > dx)
      surface_pos[0] = dx;
    else if (pos[0] < -dx)
      surface_pos[0] = -dx;
  }

  vector_to_surface = surface_pos - pos;

  return true;
}
