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

using namespace std;

//==============================================================
// Tetrahedron
//==============================================================
Tetrahedron::Tetrahedron(Physics * physics, Scalar radius, Scalar mass)
:
Object(physics),
m_collision_body(this)
{
  TRACE_FILE_IF(ONCE_1)
    TRACE("Creating tetrahedron\n");
  set_mass(mass);
  Scalar I = 0.4f * mass * radius * radius; // assume a sphere
  set_body_inertia(I, I, I);

  int mesh_num = 16;
  int num_edge_samples = 6;

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

  edge_points.resize(4);
  edge_points[0] = Position( 0.0f,  0.0f,  radius); // top
  edge_points[1] = m3beta(109.5f) * edge_points[0];
  edge_points[2] = m3gamma(-120.0f) * edge_points[1];
  edge_points[3] = m3gamma(120.0f) * edge_points[1];

  points = edge_points;

  m_triangles.resize(4);
  m_triangles[0] = Collision_triangle(edge_points[0], edge_points[1], edge_points[3]);
  m_triangles[1] = Collision_triangle(edge_points[0], edge_points[3], edge_points[2]);
  m_triangles[2] = Collision_triangle(edge_points[0], edge_points[2], edge_points[1]);
  m_triangles[3] = Collision_triangle(edge_points[1], edge_points[2], edge_points[3]);

  edges.resize(6);
  edges[0] = Collision_body_edge(0, 1, num_edge_samples);
  edges[1] = Collision_body_edge(0, 2, num_edge_samples);
  edges[2] = Collision_body_edge(0, 3, num_edge_samples);
  edges[3] = Collision_body_edge(1, 2, num_edge_samples);
  edges[4] = Collision_body_edge(2, 3, num_edge_samples);
  edges[5] = Collision_body_edge(3, 1, num_edge_samples);

  m_collision_body.initialise(mesh_num, mesh_num, mesh_num, 
                              1.1f,
                              m_triangles,
                              points,
                              edge_points,
                              edges);
}

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

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

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