/*
 * Collision-detection function collection
 *
 * Copyright (C) 2006-2010  Alejandro Valenzuela Roca, <lanjoe9 at mexinetica NOSPAMPLEASE dot com>
 *
 * This file is part of MotorJ, a free framework for videogame development.
 * <http://wiki.lidsol.net/wiki/index.php?title=MotorJ >
 * <http://motorj.mexinetica.com/ >
 *
 * MotorJ is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * MotorJ is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with MotorJ.  If not, see <http://www.gnu.org/licenses/>.
 *
 *
 * Real-Time Rendering by Tomas Akenine-Möller and Eric Haines was used as
 * a reference to implement some of the algorithms.
 *
 */

/*
 * "Pixel por Pixel, polígono por polígono"
 *  - Colisión por el bien de todos
 *
 */


#include "collisions.h"


short col_aabb_aabb(vector3 & a_min, vector3 & a_max,
		  vector3 & b_min, vector3 & b_max){
  return !((a_min.x > b_max.x) || (b_min.x > a_max.x) ||
	       (a_min.y > b_max.y) || (b_min.y > a_max.y) ||
	       (a_min.z > b_max.z) || (b_min.z > a_max.z)) ;
}


short col_aabb_pt(vector3 & a_min, vector3 & a_max, vector3 &pt)
{
  return ((a_min.x <= pt.x) && (a_min.y <= pt.y) &&
	  (a_min.z <= pt.z) && (a_max.x >= pt.x) &&
	  (a_max.y >= pt.y) && (a_max.z >= pt.z));
}

//int col_obb_obb(vector3 c_a, vector3 medidas_a, vector3 dirs_a[3],
//		vector3 c_b, vector3 medidas_b, vector3 dirs_b[3]){
short col_obb_obb(obb &a, obb &b)
{
  /*
     Implementación del método de detección de colisiones entre
     cajas orientadas, descrito en Real-Time Rendering (Tomas Möller, Eric Haines)

     El algoritmo debe recibir como primer parámetro el centro de la caja A,
     como segundo parámetro, un vértice cualquiera de la caja A (en X, Y, Z absolutos,
     es decir, referenciados con el origen del mundo) y luego recibe los datos
     equivalentes de la caja B.

  */
  //caja a;
  //caja b;
  float r_a;
  float r_b;

  int r;

  //vector3 tmp;

  vector3 l;
  vector3 t;


  /*
     Obtener el vector entre los centros de las cajas
     (Para ahorrar operaciones esto se hace de una vez)
  */

  vec_copy(b.c,&t);
  vec_sum(-1,a.c,&t);

  /*
     Según el libro, se deben realizar 15 pruebas que son más o menos las mismas:
     - Contra los ejes de a
     - Contra los ejes de b
     - Contra los ejes "cruzados" de a con b, en todas sus combinaciones
  */
  /*
    NOTA: hay maneras de hacer esto más eficientes y descritas por el libro
  */
  for (r = 0; r < 15; r++){
    switch (r){
      /* Ejes de la primer caja */
    case 0:
      vec_copy(a.u,&l);
      break;
    case 1:
      vec_copy(a.v,&l);
      break;
    case 2:
      vec_copy(a.w,&l);
      break;
      /* Ejes de la segunda caja */
    case 3:
      vec_copy(b.u,&l);
      break;
    case 4:
      vec_copy(b.v,&l);
      break;
    case 5:
      vec_copy(b.w,&l);
      break;
      /* Ejes "cruzados" */
    case 6:
      cross(a.u,b.u,&l);
      break;
    case 7:
      cross(a.u,b.v,&l);
      break;
    case 8:
      cross(a.u,b.w,&l);
      break;
    case 9:
      cross(a.v,b.u,&l);
      break;
    case 10:
      cross(a.v,b.v,&l);
      break;
    case 11:
      cross(a.v,b.w,&l);
      break;
    case 12:
      cross(a.w,b.u,&l);
      break;
    case 13:
      cross(a.w,b.v,&l);
      break;
    case 14:
      cross(a.w,b.w,&l);
      break;
    }


    /* Establecer los "radios" */
    r_a = (a.Hu * fabs(dot(a.u, l))) +
      (a.Hv * fabs(dot(a.v, l))) + (a.Hw * fabs(dot(a.w, l)));

    r_b = (b.Hu * fabs(dot(b.u, l))) +
      (b.Hv * fabs(dot(b.v, l))) + (b.Hw * fabs(dot(b.w, l)));

    //printf("rA: %f, rB: %f, l:%f\n",r_a,r_b,dot(t,l));

    /*
       Obtener el vector entre los centros de las cajas
       (Para ahorrar operaciones esto se hace una sola vez al principio
       pero el algoritmo lo menciona en este punto )
    */
    //vec_copy(b.c,&t);
    //vec_sum(-1,a.c,&t);

    /*
       Establecer la proyección de las distancias en el eje contra el que se compara
    */

    if (fabs(dot(t,l)) > r_a + r_b){
      /* Se encontró una proyección separada -> no hay colisión !!*/
      return COLR_NOCOLLISION;
    }

  }
  /* Si pasó todas las pruebas quiere decir que hubo colisión.
     Qué rudo.*/

  return COLR_OVERLAP;
}


short col_sph_sph(vector3 sphcen0, float r0,
				  vector3 sphcen1, float r1, vector3 * sph0_sph1)
{
	// If the distance between the two sphere centers is lower than
	// the sum of their radii, a collision occurs.
	vector3 tmp;

	memcpy(&tmp, &sphcen0, sizeof(vector3));

	vec_sum(-1, sphcen1, &tmp);

	if (sph0_sph1)
		memcpy(sph0_sph1, &tmp, sizeof(vector3));

	//printf("dist: %f\n", vec_norm (tmp));
	return 0 > ((vec_norm(tmp) - r0) - r1);
}

short col_ray_tr(vector3 & ray_origin, vector3 * ray_dir, triangle & tr, vector3 * projection)
{
	vector3 e1;
	vector3 e2;
	vector3 p;
	vector3 q;
	//vector3 tmp;

	float a;
	float f;
	float u;
	float v;
	float t;
	vector3 s;
	int eraNula = 0;

	vec_copy(* tr.v1, &e1);
	vec_copy(* tr.v2, &e2);

	vec_sum(-1, * tr.v0, &e1);
	vec_sum(-1, * tr.v0, &e2);

	if (ray_dir == NULL)
	{
		ray_dir = new vector3;
		eraNula = 1;
		cross(e1, e2, ray_dir);
	}

	cross(*ray_dir, e2, &p);

	a = dot(e1, p);

	// Verify whether the triangle and the ray are not parallel

	if (a > -tol && a < tol)
	{
		if (eraNula) delete ray_dir;
		return COLR_NOCOLLISION;
	}

	f = 1.0 / a;
	// s = o
	vec_copy(ray_origin, &s);

	//  s = s - v0 [i.e., s = o - v0]
	vec_sum(-1, * tr.v0, &s);

	u = f * dot(s,p);

	if (u < -0.01 || u > 1.01)
	{
		//printf("d\n");
		if (eraNula) delete ray_dir;
		return COLR_NOCOLLISION;
	}

	cross (s, e1, &q);

	v = f* dot(*ray_dir,q);



	if (v < -0.01 || (u+v) > 1.01)
	{
		//printf("d2\n");
		if (eraNula) delete ray_dir;
		return 0;
	}

	t = f * dot(e2,q);

	if (projection)
	{
		vec_copy(ray_origin, projection);
		vec_sum(t, *ray_dir, projection);
	}
	
	if (fabs(t)>0.0001)
	{

		if (eraNula) delete ray_dir;

		if (t>0)
		{
			return COLR_UNDER;
		} else return COLR_OVER;

	}
	return COLR_EXACT;
}


short col_pt_aacyl(vector3 & punto, vector3 & cil_base, float h, float r, vector3 * corr)
{
	if ((fabs(punto.x - cil_base.x) < r) && (fabs(punto.z - cil_base.z) < r)){
		float hcom = punto.y - cil_base.y;
		vector3 p_cil_base;

		vec_copy(punto, &p_cil_base);
		vec_sum(-1, cil_base, &p_cil_base);
		p_cil_base.y = 0;
		normalize(&p_cil_base);
		vec_copy(cil_base, corr);
		vec_sum( r + (0.3*r), p_cil_base, corr);
		corr->y = punto.y;
		//printf("hcom: %f\n", hcom);
		return ((hcom < h) && ((hcom + AACIL_TOL) > 0));

	} else return COLR_NOCOLLISION;
}

short col_ray_sph(vector3 & ray_origin, vector3 & ray_dir, vector3 sphcen, float r, vector3* correction)
{
	

	vector3 o_c;
	float s;
	float l2;
	float r2 = r*r;
	float m2;
	float q;
	float t;
	short result;
	vec_copy(sphcen, &o_c);
	vec_sum(-1, ray_origin, &o_c); // l
	s = dot(o_c, ray_dir);
	l2 = dot(o_c, o_c);
	
	if ((s < 0) && (l2 > r2))
	{
		return COLR_NOCOLLISION;
	}
	m2 = l2 - (s*s);
	if (m2 > r) return COLR_NOCOLLISION;
	
	
	result = COLR_OVER;
	
	if (correction)
	{
		q = sqrt(r2 - m2);
		
		if (q == 0) 
			result = COLR_EXACT;
		
		if (l2 > r2) {
			t = s - q;
		} else {
			t = s + q;
		}
		vec_copy(ray_origin, correction);
		vec_sum(t, ray_dir, correction);
	}
	
	return result;
	
}


//FIXME: not yet finished
short col_tr_aabb( triangle & tr, vector3 & b_min, vector3 & b_max)
{
	vector3 b_c;
	vector3 b_hw; // a_halfwidth
	vector3 tr_t[3]; // translated triangle vertices
	//vector3 segment0;
	//vector3 segment1;
	vector3 a_min;
	vector3 a_max;

	// Calculate the AABB's center
	vec_copy(b_max, &b_c);
	vec_sum(-1, b_min, &b_c); // c = b-a;

	// Divide by two
	mul_scal (0.5, &b_c);

	// Now we've got the "halfwidths"
	vec_copy (b_c, &b_hw);

	// Finally get the center point
	vec_sum2(b_min, &b_c); // c = a + ((b-a)/2);

	// Translate the triangle's vertices
	vec_copy ( *tr.v0, &tr_t[0]);
	vec_copy ( *tr.v1, &tr_t[1]);
	vec_copy ( *tr.v2, &tr_t[2]);

	vec_sum (-1, b_c, &tr_t[0]);
	vec_sum (-1, b_c, &tr_t[1]);
	vec_sum (-1, b_c, &tr_t[2]);

	// Now we perform 13 SAT tests (that's Separating Axis Theorem)

	// 3 tests: AABB vs AABB
	// Determine the minimum AABB and do the collision test
	vec_copy (*tr.v0, &a_min);
	vec_copy (*tr.v0, &a_max);

	if (tr.v1->x < a_min.x)
		a_min.x = tr.v1->x;
	if (tr.v1->x > a_max.x)
		a_max.x = tr.v1->x;

	if (tr.v1->y < a_min.y)
		a_min.y = tr.v1->y;
	if (tr.v1->y > a_max.y)
		a_max.y = tr.v1->y;

	if (tr.v1->z < a_min.z)
		a_min.z = tr.v1->z;
	if (tr.v1->z > a_max.z)
		a_max.z = tr.v1->z;

	if (tr.v2->x < a_min.x)
		a_min.x = tr.v2->x;
	if (tr.v2->x > a_max.x)
		a_max.x = tr.v2->x;

	if (tr.v2->y < a_min.y)
		a_min.y = tr.v2->y;
	if (tr.v2->y > a_max.y)
		a_max.y = tr.v2->y;

	if (tr.v2->z < a_min.z)
		a_min.z = tr.v2->z;
	if (tr.v2->z > a_max.z)
		a_max.z = tr.v2->z;

	if (col_aabb_aabb (a_min, a_max, b_min, b_max))
	{
		// That's it. Now,

	} else return COLR_NOCOLLISION;

}