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

using namespace std;

extern bool show_deactivation;

//==============================================================
// Box
//==============================================================
Box::Box(Physics * physics, Scalar side_x, Scalar side_y, Scalar side_z, Scalar mass, bool solid)
:
Object(physics),
m_collision_body(this),
m_side_x(side_x),
m_side_y(side_y),
m_side_z(side_z)
{
  TRACE_FILE_IF(ONCE_1)
    TRACE("Creating box\n");
  set_mass(mass);
  Scalar Ix = (1.0f / 3.0f) * mass * (side_y * side_y + side_z * side_z);
  Scalar Iy = (1.0f / 3.0f) * mass * (side_x * side_x + side_z * side_z);
  Scalar Iz = (1.0f / 3.0f) * mass * (side_x * side_x + side_y * side_y);
  set_body_inertia(Ix, Iy, Iz);

  int mesh_num = 16;
  int num_edge_samples = 6;
  int num_extra_face_points_per_side = 5;

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

  Scalar dx = side_x * 0.5f;
  Scalar dy = side_y * 0.5f;
  Scalar dz = side_z * 0.5f;

  enum {TFL = 0, TBL = 1, TBR = 2, TFR = 3, BFL = 4, BBL = 5, BBR = 6, BFR = 7};
  points.resize(8);
  points[TFL] = Position( dx,  dy,  dz);
  points[TBL] = Position(-dx,  dy,  dz);
  points[TBR] = Position(-dx, -dy,  dz);
  points[TFR] = Position( dx, -dy,  dz);

  points[BFL] = Position( dx,  dy, -dz);
  points[BBL] = Position(-dx,  dy, -dz);
  points[BBR] = Position(-dx, -dy, -dz);
  points[BFR] = Position( dx, -dy, -dz);

  edge_points = points;
//  points.clear();

  edges.resize(12);
  edges[0] = Collision_body_edge(TFL, TBL, num_edge_samples);
  edges[1] = Collision_body_edge(TBL, TBR, num_edge_samples);
  edges[2] = Collision_body_edge(TBR, TFR, num_edge_samples);
  edges[3] = Collision_body_edge(TFR, TFL, num_edge_samples);

  edges[4] = Collision_body_edge(BFL, BBL, num_edge_samples);
  edges[5] = Collision_body_edge(BBL, BBR, num_edge_samples);
  edges[6] = Collision_body_edge(BBR, BFR, num_edge_samples);
  edges[7] = Collision_body_edge(BFR, BFL, num_edge_samples);

  edges[8] = Collision_body_edge(TFL, BFL, num_edge_samples);
  edges[9] = Collision_body_edge(TBL, BBL, num_edge_samples);
  edges[10] = Collision_body_edge(TBR, BBR, num_edge_samples);
  edges[11] = Collision_body_edge(TFR, BFR, num_edge_samples);

  m_triangles.resize(12);
  // top
  m_triangles[0] = Collision_triangle(edge_points[TFL], edge_points[TBL], edge_points[TBR]);
  m_triangles[1] = Collision_triangle(edge_points[TFL], edge_points[TBR], edge_points[TFR]);

  // bottom
  m_triangles[2] = Collision_triangle(edge_points[BFL], edge_points[BBR], edge_points[BBL]);
  m_triangles[3] = Collision_triangle(edge_points[BFL], edge_points[BFR], edge_points[BBR]);

  // left
  m_triangles[4] = Collision_triangle(edge_points[BFL], edge_points[BBL], edge_points[TBL]);
  m_triangles[5] = Collision_triangle(edge_points[BFL], edge_points[TBL], edge_points[TFL]);

  // right
  m_triangles[6] = Collision_triangle(edge_points[BFR], edge_points[TFR], edge_points[TBR]);
  m_triangles[7] = Collision_triangle(edge_points[BFR], edge_points[TBR], edge_points[BBR]);

  // front
  m_triangles[8] = Collision_triangle(edge_points[TFL], edge_points[BFR], edge_points[BFL]);
  m_triangles[9] = Collision_triangle(edge_points[TFL], edge_points[TFR], edge_points[BFR]);

  // back
  m_triangles[10] = Collision_triangle(edge_points[TBL], edge_points[BBR], edge_points[TBR]);
  m_triangles[11] = Collision_triangle(edge_points[TBL], edge_points[BBL], edge_points[BBR]);

  // add some extra points to the box faces
  int num = num_extra_face_points_per_side; // number of points along each side
  int n = num-1;
  int i, j, k;
  for (i = 0 ; i < num ; ++i)
  {
    for (j = 0 ; j < num ; ++j)
    {
      for (k = 0 ; k < num ; ++k)
      {
        if ( (i == 0) || (i == n )||
             (j == 0) || (j == n )||
             (k == 0) || (k == n ) )
        {
          bool on_x_end = ( (i == 0) || (i == n) );
          bool on_y_end = ( (j == 0) || (j == n) );
          bool on_z_end = ( (k == 0) || (k == n) );
          if ( (on_x_end && (!on_y_end && !on_z_end)) ||
               (on_y_end && (!on_z_end && !on_x_end)) ||
               (on_z_end && (!on_x_end && !on_y_end)) )
          {
            Scalar x = -dx + (side_x * i) / n;
            Scalar y = -dy + (side_y * j) / n;
            Scalar z = -dz + (side_z * k) / n;
            points.push_back(Position(x, y, z));
          }
        }
      }
    }
  }
  m_collision_body.initialise(mesh_num, mesh_num, mesh_num, 
                              1.1f,
                              m_triangles,
                              points,
                              edge_points,
                              edges,
                              this);
}

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

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

//==============================================================
// get_mesh_info
//==============================================================
bool Box::get_mesh_info(const Position & pos, 
                        bool & inside,
                        Vector3 & vector_to_surface) const
{
  Vector3 delta(m_side_x * 0.5f, m_side_y * 0.5f, m_side_z * 0.5f);

  Position face_point;
  Scalar dists[3];
  inside = true;

  int i;
  for (i = 0 ; i < 3 ; ++i)
  {
    if (pos[i] > delta[i])
    {
      face_point[i] = delta[i];
      inside = false;
    }
    else if (pos[i] < -delta[i])
    {
      face_point[i] = -delta[i];
      inside = false;
    }
    else
    {
      if (pos[i] > 0.0f)
        face_point[i] = delta[i];
      else
        face_point[i] = -delta[i];
    }
    dists[i] = fabs(face_point[i] - pos[i]);
  }

  int best_i = 0;
  // work out the component that leads to the closest face
  if (dists[1] < dists[best_i])
    best_i = 1;
  if (dists[2] < dists[best_i])
    best_i = 2;

  for (i = 0 ; i < 3 ; ++i)
  {
    if (i  != best_i)
    {
      face_point[i] = pos[i];
      if (face_point[i] > delta[i])
        face_point[i] = delta[i];
      else if (face_point[i] < -delta[i])
        face_point[i] = -delta[i];
    }
  }

  vector_to_surface = face_point - pos;

  return true;
}
