// Author: Hector Navarro (hnavarro@ciens.ucv.ve),
//         Juan Carlos De Abreu (jcabreur@gmail.com)

#include "aabb_tree_collision.h"
#include "../Model/Model.h"
#include "aabb_tree.h"
#include "obb.h"
#include "../Scene/scene_nodes/geometry_node_inl.h"

BEGIN_TEG_NAMESPACE();

AABBTreeCollision::AABBTreeCollision() {}

AABBTreeCollision::AABBTreeCollision(
    const SmartPointer<GeometryNode>& geometry1,
    const SmartPointer<GeometryNode>& geometry2)
	: geometry1_(geometry1), geometry2_(geometry2) {
	model_view1_ = math::transpose(geometry1_->model_view());
	model_view2_ = math::transpose(geometry2_->model_view());
}

AABBTreeCollision::~AABBTreeCollision() {}

// Function to project a point 'V' over a line 'D'.
float Project(Vec3f v, Vec3f d) {
  return dot(v, d) / dot(d, d);
}

// 
void Project(const std::vector<Vec3f>& vl, Vec3f d, float& min_, float& max_) {
  // proyecta los 8 vertices de vl sobre la recta D y deja los bordes en m y M
  min_ = 1E9f; max_ = -1E9f;
  for (int i = 0; i < 8; ++i) {
    const float t = Project(vl[i], d);
    min_ = min(min_, t);
    max_ = max(max_, t);
  }
}

// 
bool ProjectAndTry(Vec3f d, const std::vector<Vec3f>& vl1, const std::vector<Vec3f>& vl2) {
  // proyecta los 8 puntos de vl1 ylos 8 de vl2 sobre D
  // y verifica si hay un plano separador, en cuyo caso retorna falso
  float min1, min2, max1, max2;
  d.normalize();
  Project(vl1, d, min1, max1);
  Project(vl2, d, min2, max2);
  return !(max1 < min2 || max2 < min1); // plano separador encontrado => no hay colision!
}

// Method to detect collision.
bool AABBTreeCollision::Detect() { 
  // 
  const int kNumberOfGroups1 =
      static_cast<int>(geometry1_->model()->bb.roots().size());
  const int kNumberOfGroups2 =
      static_cast<int>(geometry2_->model()->bb.roots().size());
  for (int i = 0; i < kNumberOfGroups1; ++i) {
    for (int j = 0; j < kNumberOfGroups2; ++j) {
      if (Detect(geometry1_->model()->bb.roots()[i],
                 geometry2_->model()->bb.roots()[j]))
        return true;
    }
  }
  return false;
}

// Method to detect collision between two OBB.
bool AABBTreeCollision::Detect(const OBB& bb1, const OBB& bb2) {
  // 
  if (!ProjectAndTry(bb1.axis(0), bb1.vertex(), bb2.vertex())) return false;
  if (!ProjectAndTry(bb1.axis(1), bb1.vertex(), bb2.vertex())) return false;
  if (!ProjectAndTry(bb1.axis(2), bb1.vertex(), bb2.vertex())) return false;
  if (!ProjectAndTry(bb2.axis(0), bb1.vertex(), bb2.vertex())) return false;
  if (!ProjectAndTry(bb2.axis(1), bb1.vertex(), bb2.vertex())) return false;
  if (!ProjectAndTry(bb2.axis(2), bb1.vertex(), bb2.vertex())) return false;

  // faltan otros 9, inter bb!	
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      if (bb1.axis(i) == bb2.axis(j)) continue;
      const Vec3f d = cross(bb1.axis(i), bb2.axis(j));
      if (!ProjectAndTry(d, bb1.vertex(), bb2.vertex())) return false;
    }
  }
  return true;
}

// Method to detect collision between two AABBNodes.
bool AABBTreeCollision::Detect(const SmartPointer<AABBNode>& node1,
                               const SmartPointer<AABBNode>& node2) {
  if (Detect(node1->bounding_box(), node2->bounding_box())) {
    // 
    const bool kIsLeaf1 = node1->IsLeaf();
    const bool kIsLeaf2 = node2->IsLeaf();
	
    if (kIsLeaf1 && kIsLeaf2)
      return true; // Check triangle to triangle.
    if (kIsLeaf1 && !kIsLeaf2)
      return Detect(node1, node2->left()) || Detect(node1, node2->right());
    if (!kIsLeaf1 && kIsLeaf2)
      return Detect(node1->left(), node2) || Detect(node1->right(), node2);

    return Detect(node1->left(), node2->left()) ||
           Detect(node1->left(), node2->right()) ||
           Detect(node1->right(), node2->left()) ||
           Detect(node1->right(), node2->right());
  }
  return false;
}

std::vector<Vec3f> Transform(Vec3f& center, const Mat4f& tr) {
  std::vector<Vec3f> axis;
  axis.push_back(math::vec3(tr * Vec4f(1.0f, 0.0f, 0.0f, 1.0f)));
  axis.push_back(math::vec3(tr * Vec4f(0.0f, 1.0f, 0.0f, 1.0f)));
  axis.push_back(math::vec3(tr * Vec4f(0.0f, 0.0f, 1.0f, 1.0f)));
  center = math::vec3(tr * Vec4f(0.0f, 0.0f, 0.0f, 1.0f));
  axis[0] -= center;
  axis[1] -= center;
  axis[2] -= center;
  return axis;
}

// Function to check if L is a separating axis between two AABB.
bool SeparatingAxis(
    const Vec3f& size1,
    const Vec3f& size2,
	const std::vector<Vec3f>& axis1,
	const std::vector<Vec3f>& axis2,
    const Vec3f& center1,
    const Vec3f& center2,
    const Vec3f& L) {

  const Vec3f T = center2 - center1;
  float ac = 0.0f;
  for (int i = 0; i < 3; ++i) {
    ac += fabs(dot(axis1[i] * (size1[i] * 0.5f), L));
	ac += fabs(dot(axis2[i] * (size2[i] * 0.5f), L));
  }
  const float TL = (float) fabs(dot(T, L));
  return TL > ac;
}

bool AABBTreeCollision::Detect(const AABB& bb1, const AABB& bb2) {
  // 
  Vec3f center1, center2;
  const std::vector<Vec3f> axis1 = Transform(center1, model_view1_);
  const std::vector<Vec3f> axis2 = Transform(center2, model_view2_);

  const Vec3f size1 = bb1.GetSize();
  const Vec3f size2 = bb2.GetSize();

  for (int i = 0; i < 3; ++i) {
    if (SeparatingAxis(size1, size2, axis1, axis2, center1, center2, axis1[i]))
      return false;
    if (SeparatingAxis(size1, size2, axis1, axis2, center1, center2, axis2[i]))
      return false;
	for (int j = 0; j < 3; ++j) {
      if (SeparatingAxis(size1,
                         size2,
                         axis1,
                         axis2,
                         center1,
                         center2,
                         cross(axis1[i], axis2[j])))
        return false;
	}
  }
  return true;
}

END_TEG_NAMESPACE();
