#include "coll_detect.hpp"
#include "rigid_body.hpp"
#include "log_trace.hpp"
#include "my_assert.hpp"

using namespace std;

bool coll_detect_use_one_coll_per_edge_for_body = false;

//==============================================================
// detect_body_collision
//==============================================================
static void detect_body_collision(Collision_body * body0,
                                  Collision_body * body1,
                                  std::vector<Collision_info> & collisions)
{
  // transfrom body1 into body0 frame, and detect by checking the
  // points of body 1 against the mesh of body 0
  
  const int num_points = body1->world_points().size();
  int i, j;
  
  Position pos0, pos1;
  Scalar r0, r0_squared;
  body0->get_bounding_sphere(pos0, r0);
  r0_squared = r0 * r0;
  
  // returned in the collision check
  Vector3 dir;
  Scalar dist;
  
  //============= the points ===========================
  for (i = 0 ; i < num_points ; ++i)
  {
    const Position & orig_pos1 = body1->world_points()[i];
    
    if ((orig_pos1 - pos0).mag2() < r0_squared)
    {
      // need to transform into body 0 frame...
      const Position pos1 = body0->get_inv_orientation() * 
        (orig_pos1 - body0->get_position());
      
      if (body0->get_point_info(pos1, dir, dist, false))
      {
        // note that dir is the normal direction on body 0. Collision
        // handling wants the opposing normal.
        
        // Got intersection - return values were in body0 reference
        // frame, so need to put into world coords
        const Vector3 normal = body0->get_orientation() * dir;
        const Position mid_point = orig_pos1 + normal * (0.5f * dist);
        
        collisions.push_back(
          Collision_info(body0, body1,
                         -normal,
                         mid_point, 
                         dist));
        body0->collisions().push_back(collisions.size() - 1);
        body1->collisions().push_back(collisions.size() - 1);
      }
    }
  }
  
  //============= the edges ===========================
  const int num_edges = body1->edges().size();
  for (j = 0 ; j < num_edges ; ++j)
  {
    const Collision_body_edge & edge = body1->edges()[j];
    const int num_points = edge.num_samples;
    const Position & pos1_start = body1->world_edge_points()[edge.i0];
    const Position & pos1_end = body1->world_edge_points()[edge.i1];
    const Vector3 delta = pos1_end - pos1_start;
    
    if (coll_detect_use_one_coll_per_edge_for_body)
    {
      // only record the collision from the depest point
      Vector3 best_normal;
      Position best_pos;
      Scalar best_depth = -1000.0f;
      
      for (i = 0 ; i < num_points ; ++i)
      {
        const Position orig_pos1 = pos1_start + (i * delta) / num_points;
        
        if ((orig_pos1 - pos0).mag2() < r0_squared)
        {
          // need to transform into body 0 frame...
          Position pos1 = body0->get_inv_orientation() * 
            (orig_pos1 - body0->get_position());
          
          if (body0->get_point_info(pos1, dir, dist, false))
          {
            if (dist > best_depth)
            {
              // note that dir is the normal direction on body
              // 0. Collision handling wants the opposing normal.
              
              // Got intersection - return values were in body0
              // reference frame, so need to put into world coords
              best_normal = body0->get_orientation() * dir;
              best_pos = orig_pos1 + best_normal * (0.5f * dist);
              best_depth = dist;
            }
          }
          if (best_depth >= 0.0f)
          {
            collisions.push_back(
              Collision_info(body0, body1,
                             -best_normal,
                             best_pos, 
                             best_depth));
            body0->collisions().push_back(collisions.size() - 1);
            body1->collisions().push_back(collisions.size() - 1);
          }
        }
      }
    }
    else // allow multiple contacts per edge
    {
      for (i = 0 ; i < num_points ; ++i)
      {
        const Position orig_pos1 = pos1_start + ((Scalar) i / num_points) * delta;
        
        if ((orig_pos1 - pos0).mag2() < r0_squared)
        {
          // need to transform into body 0 frame...
          Position pos1 = body0->get_inv_orientation() * 
            (orig_pos1 - body0->get_position());
          
          if (body0->get_point_info(pos1, dir, dist, false))
          {
            // note that dir is the normal direction on body
            // 0. Collision handling wants the opposing normal.
            
            // Got intersection - return values were in body0
            // reference frame, so need to put into world coords
            const Vector3 normal = body0->get_orientation() * dir;
            const Position mid_point = orig_pos1 + normal * (0.5f * dist);
            
            collisions.push_back(
              Collision_info(body0, body1,
                             -normal,
                             mid_point, 
                             dist));
            body0->collisions().push_back(collisions.size() - 1);
            body1->collisions().push_back(collisions.size() - 1);
          }
        }
      }
    }
  }
}

//==============================================================
// is_collidable
//==============================================================
inline bool is_collidable(Collision_body * body, std::vector<Collision_body *> & nc)
{
  // most of the time there won't be any non-collidables....
  if (nc.empty())
    return true;
  const int num = nc.size();
  int i;
  // or if there are, then only a few, so just loop
  for (i = 0 ; i < num ; ++i)
  {
    if (nc[i] == body)
      return false;
  }
  return true;
}

//==============================================================
// detect_all_collisions
//==============================================================
void detect_all_collisions(
  const std::vector<Collision_body *> & collision_bodies,
  std::vector<Collision_info> & collisions)
{
  int num_bodies = collision_bodies.size();

  int i, j;

  // heightmaps first
  for (i = 0 ; i < num_bodies ; ++i)
  {
    if ( (!collision_bodies[i]->rigid_body()->get_immovable()) &&
        (collision_bodies[i]->rigid_body()->get_activity_state() == Rigid_body::ACTIVE) )
    {
    collision_bodies[i]->calculate_world_properties();
    }
  }

  // body-body Note the world properties are already calculated
  Position pos0, pos1;
  Scalar r0, r1;
  for (i = 0 ; i < num_bodies ; ++i)
  {
    // detect up to i - double collision check in the inner part of this loop
    for (j = 0 ; j < i ; ++j)
    {
//      if (i != j)
      {
        if (!collision_bodies[i]->rigid_body()->get_immovable() ||
            !collision_bodies[j]->rigid_body()->get_immovable())
        {
          if ( (collision_bodies[i]->rigid_body()->get_activity_state() == Rigid_body::ACTIVE) ||
               (collision_bodies[j]->rigid_body()->get_activity_state() == Rigid_body::ACTIVE) )
          {
            // both bodies should be in each others lists... so just check one
            std::vector<Collision_body *> & nc_i = 
              collision_bodies[i]->non_collidables();
            if (is_collidable(collision_bodies[j], nc_i))
            {
              collision_bodies[i]->get_bounding_sphere(pos0, r0);
              collision_bodies[j]->get_bounding_sphere(pos1, r1);
              if ((pos1 - pos0).mag2() < ((r0 + r1) * (r0 + r1)))
              {
                // only continue if the bounding sphere of one intersects 
                // the bounding box of the other
                const Position posi_in_bodyj_frame = 
                  collision_bodies[j]->get_inv_orientation() * 
                  (pos0 - collision_bodies[j]->get_position());
              
                Scalar sqr_dist_bodyi_to_bodyj_box = 
                  collision_bodies[j]->get_sqr_distance_to_bounding_box(
                    posi_in_bodyj_frame);
              
                if (sqr_dist_bodyi_to_bodyj_box < (r0 * r0))
                {
                  const Position posj_in_bodyi_frame = 
                    collision_bodies[i]->get_inv_orientation() * 
                    (pos1 - collision_bodies[i]->get_position());
                
                  Scalar sqr_dist_bodyj_to_bodyi_box = 
                    collision_bodies[i]->get_sqr_distance_to_bounding_box(
                      posj_in_bodyi_frame);
                
                  if (sqr_dist_bodyj_to_bodyi_box < (r1 * r1))
                  {
                    detect_body_collision(collision_bodies[i], 
                                          collision_bodies[j], collisions);
                    detect_body_collision(collision_bodies[j], 
                                          collision_bodies[i], collisions);
                  }
                }
              }
            }
          }
        }
      }
    }
  }
}

//==============================================================
// detect_collisions_with_body
//==============================================================
void detect_frozen_collisions_with_body(Collision_body * collision_body,
                                        const std::vector<Collision_body *> & collision_bodies,
                                        std::vector<Collision_info> & collisions)
{
  if (collision_body->rigid_body()->get_immovable())
    return;

  int num_bodies = collision_bodies.size();
  
  int j;
  
  collision_body->calculate_world_properties();
 
  // body-body Note the world properties are already calculated
  Position pos0, pos1;
  Scalar r0, r1;

  collision_body->get_bounding_sphere(pos0, r0);

  for (j = 0 ; j < num_bodies ; ++j)
  {
    if (collision_bodies[j]->rigid_body() != collision_body->rigid_body())
    {
      if ( (collision_bodies[j]->rigid_body()->get_activity_state() == Rigid_body::FROZEN ) ||
           (!collision_bodies[j]->rigid_body()->get_immovable()) )
      {
        // both bodies should be in each others lists... so just check one
        std::vector<Collision_body *> & nc_i = 
          collision_body->non_collidables();
        if (is_collidable(collision_bodies[j], nc_i))
        {
          collision_bodies[j]->get_bounding_sphere(pos1, r1);
          if ((pos1 - pos0).mag2() < ((r0 + r1) * (r0 + r1)))
          {
            // only continue if the bounding sphere of one intersects 
            // the bounding box of the other
            const Position posi_in_bodyj_frame = 
              collision_bodies[j]->get_inv_orientation() * 
              (pos0 - collision_bodies[j]->get_position());
        
            Scalar sqr_dist_bodyi_to_bodyj_box = 
              collision_bodies[j]->get_sqr_distance_to_bounding_box(
                posi_in_bodyj_frame);
        
            if (sqr_dist_bodyi_to_bodyj_box < (r0 * r0))
            {
              const Position posj_in_bodyi_frame = 
                collision_body->get_inv_orientation() * 
                (pos1 - collision_body->get_position());
          
              Scalar sqr_dist_bodyj_to_bodyi_box = 
                collision_body->get_sqr_distance_to_bounding_box(
                  posj_in_bodyi_frame);
          
              if (sqr_dist_bodyj_to_bodyi_box < (r1 * r1))
              {
                detect_body_collision(collision_body, 
                                      collision_bodies[j], collisions);
                detect_body_collision(collision_bodies[j], 
                                      collision_body, collisions);
              }
            }
          }
        }
      }
    }
  }
}
