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

using namespace std;

//==============================================================
// Ellipsoid
//==============================================================
Ellipsoid::Ellipsoid(Physics * physics, 
                     Scalar diameter_x, Scalar diameter_y, Scalar diameter_z,
                     int slices, int stacks,
                     Scalar mass)
:
Object(physics),
m_collision_body(this),
m_radius_x(diameter_x * 0.5f),
m_radius_y(diameter_y * 0.5f),
m_radius_z(diameter_z * 0.5f),
m_collision_enabled(true)
{
  TRACE_FILE_IF(ONCE_1)
    TRACE("Creating Ellipsoid\n");
  set_mass(mass);
  Scalar Ix = (1.0f / 5.0f) * mass * (m_radius_y * m_radius_y + m_radius_z * m_radius_z);
  Scalar Iy = (1.0f / 5.0f) * mass * (m_radius_x * m_radius_x + m_radius_z * m_radius_z);
  Scalar Iz = (1.0f / 5.0f) * mass * (m_radius_x * m_radius_x + m_radius_y * m_radius_y);
  set_body_inertia(Ix, Iy, Iz);

  int mesh_num = 10;

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

  int slice, stack;
  // do the top and bottom separately since they have degenerate triangles
  for (stack = 0 ; stack < stacks-1 ; ++stack)
  {
    for (slice = 0 ; slice < slices ; ++slice)
    {
      // four points of interest, labelled from left to right
      // "longitude"
      Scalar long0 = ((0.0f + slice) / (slices - 1.0f)) * 360.0f;
      Scalar long1 = ((0.0f + slice) / (slices - 1.0f)) * 360.0f;
      Scalar long2 = ((1.0f + slice) / (slices - 1.0f)) * 360.0f;
      Scalar long3 = ((1.0f + slice) / (slices - 1.0f)) * 360.0f;
      // "latitude"
      Scalar lat0 = -90.0f + ((0.0f + stack) / (stacks - 1.0f)) * 180.0f;
//      Scalar lat2 = lat0;
      Scalar lat1 = -90.0f + ((1.0f + stack) / (stacks - 1.0f)) * 180.0f;
//      Scalar lat3 = lat1;

      // the "reduced radius"
      Scalar rp0 = cos_deg(lat0);
      Scalar rp1 = cos_deg(lat1);
      Scalar rp2 = rp0;
      Scalar rp3 = rp1;

      Scalar sin_lat0 = sin_deg(lat0);
      Scalar sin_lat1 = sin_deg(lat1);
      Scalar sin_lat2 = sin_lat0;
      Scalar sin_lat3 = sin_lat1;

      Position pos[4] = {
        Position(rp0 * cos_deg(long0), rp0 * sin_deg(long0), sin_lat0),
        Position(rp1 * cos_deg(long1), rp1 * sin_deg(long1), sin_lat1),
        Position(rp2 * cos_deg(long2), rp2 * sin_deg(long2), sin_lat2),
        Position(rp3 * cos_deg(long3), rp3 * sin_deg(long3), sin_lat3) 
      };

      int j;
      for (j = 0 ; j < 4 ; ++j)
      {
        pos[j][0] *= m_radius_x;
        pos[j][1] *= m_radius_y;
        pos[j][2] *= m_radius_z;
      }

      if (stack == 0)
      {
        if (slice == 0)
          points.push_back(pos[0]);
        m_triangles.push_back(Collision_triangle(pos[2], pos[3], pos[1]));
      }
      else if (stack == stacks - 2)
      {
        points.push_back(pos[0]);
        if (slice == 0)
          points.push_back(pos[1]);
        m_triangles.push_back(Collision_triangle(pos[0], pos[2], pos[1]));
      }
      else
      {
        points.push_back(pos[0]);

        m_triangles.push_back(Collision_triangle(pos[0], pos[2], pos[1]));
        m_triangles.push_back(Collision_triangle(pos[2], pos[3], pos[1]));
      }
    }
  }
  m_collision_body.initialise(mesh_num, mesh_num, mesh_num, 
                              1.1f,
                              m_triangles,
                              points,
                              edge_points,
                              edges,
                              this);
  m_colour_front = Vector3(1.0f, ranged_random(0.2f, 1.0f), 0.0f);
  m_colour_back = Vector3(0.0f, ranged_random(0.2f, 1.0f), 1.0f);

}

//==============================================================
// Ellipsoid
// based on an icosahedron
//==============================================================
Ellipsoid::Ellipsoid(Physics * physics, 
                     Scalar diameter_x, Scalar diameter_y, Scalar diameter_z,
                     Scalar mass)
:
Object(physics),
m_collision_body(this),
m_radius_x(diameter_x),
m_radius_y(diameter_y),
m_radius_z(diameter_z)
{
  TRACE_FILE_IF(ONCE_1)
    TRACE("Creating Ellipsoid\n");
  set_mass(mass);
  Scalar Ix = (1.0f / 5.0f) * mass * (m_radius_y * m_radius_y + m_radius_z * m_radius_z);
  Scalar Iy = (1.0f / 5.0f) * mass * (m_radius_x * m_radius_x + m_radius_z * m_radius_z);
  Scalar Iz = (1.0f / 5.0f) * mass * (m_radius_x * m_radius_x + m_radius_y * m_radius_y);
  set_body_inertia(Ix, Iy, Iz);

  int mesh_num = 12;

  assert1(!"not implemented\n");
}

//==============================================================
// get_bounding_sphere
//==============================================================
void Ellipsoid::get_bounding_sphere(Position & pos, Scalar & radius)
{
  m_collision_body.get_bounding_sphere(pos, radius);
}

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

//==============================================================
// get_mesh_info
//==============================================================
bool Ellipsoid::get_mesh_info(const Position & pos, 
                              bool & inside,
                              Vector3 & vector_to_surface) const
{
  Ellipsoid_shape e(Vector3(m_radius_x, m_radius_y, m_radius_z));
  Position closest;

  Scalar dist = distance_sqr(pos, e, closest);

  if (dist < 0.0f)
    TRACE("ow - unable to get distance to ellipsoid\n");

  vector_to_surface = closest - pos;

  Scalar v = (pos[0] / m_radius_x) * (pos[0] / m_radius_x) + 
             (pos[1] / m_radius_y) * (pos[1] / m_radius_y) + 
             (pos[2] / m_radius_z) * (pos[2] / m_radius_z);
  if (v > 1.0f)
    inside = false;
  else 
    inside = true;

  return true;
}
