/*
 * Copyright (c) 2008-2009 Bill Whitacre http://rampancy.g0dsoft.com
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
*/

#include "collision.h"

rat_coll_query_fn rat_coll_query_fn_arr[rat_num_hull_types*rat_num_hull_types*2];
int rat_is_coll_query_fn_arr_init=0;

int rat_always_collide(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons) {return 1;}
int rat_never_collide(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)  {return 0;}

static int rat_circle_query(rat_block_allocator *bal,rat_contact_array **cons,vector2 center_a,rat_real radius_a,vector2 center_b,rat_real radius_b)
{
	int numcons;
	rat_real pen,dist,radii;

	dist=vector2_distance(center_a,center_b);
	dist=dist==0?0.001:dist;
	radii=radius_a+radius_b;
	pen=dist-radii;

	if (pen<=0.0)
	{
		if (cons)
		{
			rat_contact *con;
			*cons=rat_alloc_contact_array(bal,1);
			con=rat_indexed_contact(*cons,0);
			con->idvala=0;
			con->idvalb=0;
			con->point=vector2_add(center_a,vector2_multf(vector2_sub(center_b,center_a),0.5+(radius_a-0.5*radii)/dist));
			con->normal=vector2_sub(center_a,center_b);
			if (con->normal.x==0&&con->normal.y==0)
			{
				con->normal.x=1;
				con->normal.y=0;
			}
			else
				vector2_normalize(&(con->normal));
			con->penetration=pen;
		}
		return 1;
	}

	return 0;
}


int rat_coll_particle_circle(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	rat_hull_particle *hull_a=(rat_hull_particle *)a;
	rat_hull_circle *hull_b=(rat_hull_circle *)b;

	int rtnval=rat_circle_query(bal,cons,hull_a->point,0.0,hull_b->center,hull_b->radius);
	if (cons&&rtnval)
	{
		rat_indexed_contact(*cons,0)->hull_a=a;
		rat_indexed_contact(*cons,0)->hull_b=b;
	}
	return rtnval;
}

int rat_coll_particle_poly(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	int returnvalue=rat_coll_poly_particle(bal,b,a,cons);
	if (returnvalue&&cons) rat_invert_contacts(*cons);
	return returnvalue;
}

int rat_coll_circle_circle(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	rat_hull_circle *hull_a=(rat_hull_circle *)a;
	rat_hull_circle *hull_b=(rat_hull_circle *)b;

	int rtnval=rat_circle_query(bal,cons,hull_a->center,hull_a->radius,hull_b->center,hull_b->radius);
	if (rtnval&&cons)
	{
		rat_indexed_contact(*cons,0)->hull_a=a;
		rat_indexed_contact(*cons,0)->hull_b=b;
	}
	return rtnval;
}

int rat_coll_circle_poly(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	int returnvalue=rat_coll_poly_circle(bal,b,a,cons);
	if (returnvalue&&cons) rat_invert_contacts(*cons);
	return returnvalue;
}

int rat_coll_circle_particle(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	rat_hull_circle *hull_a=(rat_hull_circle *)a;
	rat_hull_particle *hull_b=(rat_hull_particle *)b;

	int rtnval=rat_circle_query(bal,cons,hull_a->center,hull_a->radius,hull_b->point,0.0);
	if (rtnval&&cons)
	{
		rat_indexed_contact(*cons,0)->hull_a=a;
		rat_indexed_contact(*cons,0)->hull_b=b;
	}
	return rtnval;
}

int rat_coll_poly_circle(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	register unsigned int i;

	unsigned int min_interval_i=0;
	rat_real min_interval;

	rat_hull_polygon *polygon=(rat_hull_polygon *)a;
	rat_hull_circle *circle=(rat_hull_circle *)b;

	rat_real dist_a,dist_b,dist_n;

	// get the minimum interval / normal
	min_interval=vector2_dot(polygon->world_axes[0].normal,circle->center)-polygon->world_axes[0].dist-circle->radius;
	for (i=0; i<polygon->numverts; i++)
	{
		rat_real dist=vector2_dot(polygon->world_axes[i].normal,circle->center)-polygon->world_axes[i].dist-circle->radius;
		if (dist>0.0)
			return 0;
		else if (dist>min_interval)
		{
			min_interval=dist;
			min_interval_i=i;
		}
	}

	// get the contact
	dist_a=vector2_cross(polygon->world_axes[min_interval_i].normal,polygon->world_verts[min_interval_i]);
	dist_b=vector2_cross(polygon->world_axes[min_interval_i].normal,polygon->world_verts[(min_interval_i+1)%polygon->numverts]);
	dist_n=vector2_cross(polygon->world_axes[min_interval_i].normal,circle->center);

	if (dist_n<dist_b)
	{
		int rtn=rat_circle_query(bal,cons,polygon->world_verts[(min_interval_i+1)%polygon->numverts],0.0,circle->center,circle->radius);
		if (rtn&&cons)
		{
			rat_contact *con=rat_indexed_contact(*cons,0);
			con->hull_a=a;
			con->hull_b=b;
			con->idvala=0;
			con->idvalb=1;
		}
		return rtn;
	}
	else if (dist_n<dist_a)
	{
		rat_contact *con;

		if (min_interval<0.0)
		{
			if (cons)
			{
				*cons=rat_alloc_contact_array(bal,1);
				con=rat_indexed_contact(*cons,0);

				con->point=vector2_sub(circle->center,vector2_multf(polygon->world_axes[min_interval_i].normal,circle->radius+min_interval/2.0));
				con->normal=vector2_negative(polygon->world_axes[min_interval_i].normal);
				con->penetration=min_interval;

				con->hull_a=a;
				con->hull_b=b;
				con->idvala=1;
				con->idvalb=2;
			}
			return 1;
		}
		return 0;
	}
	else
	{
		int rtn=rat_circle_query(bal,cons,polygon->world_verts[min_interval_i],0.0,circle->center,circle->radius);
		if (rtn&&cons)
		{
			rat_contact *con=rat_indexed_contact(*cons,0);
			con->hull_a=a;
			con->hull_b=b;
			con->idvala=2;
			con->idvalb=3;
		}
		return rtn;
	}
}

static int minimum_separating_axis(rat_hull_polygon *poly,rat_polygon_axis *axes,unsigned int numaxes,rat_real *minsepaxis)
{
	register unsigned int i;
	register int min_index=0;
	register rat_real min=rat_polygon_axis_value(poly->world_verts,poly->numverts,axes[0]);
	if (min>0.0) return -1;

	for (i=1; i<numaxes; i++)
	{
		register rat_real dist=rat_polygon_axis_value(poly->world_verts,poly->numverts,axes[i]);

		if (dist>0.0)
			return -1;
		else if (dist>min)
		{
			min=dist;
			min_index=i;
		}
	}

	*minsepaxis=min;
	return min_index;
}

static void contact_verts(rat_contact_array **cons,rat_hull_polygon *poly_a,
							  rat_hull_polygon *poly_b,vector2 normal,rat_real normal_coef,
							  rat_real distance)
{
	register unsigned int i,j;

	vector2_timesf(&normal,normal_coef);

	for (i=0; i<poly_a->numverts; i++)
	{
		vector2 point=poly_a->world_verts[i];
		if (rat_hull_polygon_point_query((rat_hull *)poly_b,point))
		{
			rat_contact *con=rat_add_array_contact(*cons);
			con->hull_a=(rat_hull *)poly_a;
			con->hull_b=(rat_hull *)poly_b;
			con->idvala=i;
			con->idvalb=1;
			con->point=point;
			con->normal=normal;
			con->penetration=distance;
		}
	}

	for (i=0; i<poly_b->numverts; i++)
	{
		vector2 point=poly_b->world_verts[i];
		if (rat_hull_polygon_point_query((rat_hull *)poly_a,point))
		{
			rat_contact *con=rat_add_array_contact(*cons);
			con->hull_a=(rat_hull *)poly_a;
			con->hull_b=(rat_hull *)poly_b;
			con->idvala=i;
			con->idvalb=2;
			con->point=point;
			con->normal=normal;
			con->penetration=distance;
		}
	}
}

static void contact_crossings(rat_contact_array **cons,rat_hull_polygon *poly_a,
								  rat_hull_polygon *poly_b,vector2 normal,rat_real normal_coef,
								  rat_real distance)
{
	register unsigned int i,j;

	vector2 *crossings;
	vector2 crossings_centroid;

	register unsigned int numcrosses=0;
	crossings=(vector2 *)rat_alloc(poly_a->numverts*poly_b->numverts*2*sizeof(vector2));

	vector2_timesf(&normal,normal_coef);

	for (i=0; i<poly_a->numverts; i++)
	{
		line2 edge_a;
		edge_a.a=poly_a->world_verts[i];
		edge_a.b=poly_a->world_verts[(i+1)%poly_a->numverts];

		for (j=0; j<poly_b->numverts; j++)
		{
			vector2 intersection;
			line2 edge_b;

			edge_b.a=poly_b->world_verts[j];
			edge_b.b=poly_b->world_verts[(j+1)%poly_b->numverts];

			if (line2_intersection(edge_a,edge_b,&intersection))
			{
				crossings[numcrosses]=intersection;
				numcrosses++;
			}
		}
	}

	if (numcrosses)
	{
		rat_contact *con=rat_add_array_contact(*cons);
		crossings_centroid=vector2_average(crossings,numcrosses);

		con->hull_a=(rat_hull *)poly_a;
		con->hull_b=(rat_hull *)poly_b;
		con->idvala=(unsigned int)poly_a;
		con->idvalb=(unsigned int)poly_a;
		con->point=crossings_centroid;
		con->normal=normal;
		con->penetration=distance;
	}

	rat_dealloc((void *)crossings);
}

/*static void contact_smallest_manifold(rat_contact_array **cons,rat_hull_polygon *poly_a,
							  rat_hull_polygon *poly_b,vector2 normal,rat_real normal_coef,
							  rat_real distance)
{
	register unsigned int i;
	rat_real mindist;
	int scm_a_i=0,lcm_a_i=0,scm_b_i=0,lcm_b_i=0;
	rat_real scm_a,lcm_a=0,scm_b,lcm_b=0;
	rat_contact *con;

	vector2_timesf(&normal,normal_coef);

	rat_polygon_point_manifold_depth(poly_b->world_verts,poly_b->world_axes,poly_b->numverts,poly_a->world_verts[0],&scm_a);
	for (i=1; i<poly_a->numverts; i++)
	{
		vector2 point=poly_a->world_verts[i];
		if (rat_polygon_point_manifold_depth(poly_b->world_verts,poly_b->world_axes,poly_b->numverts,point,&mindist)!=-1)
		{
			if (mindist<0.0)
			{
				if (mindist>scm_a)
				{
					scm_a=mindist;
					scm_a_i=i;
				}
				if (mindist<lcm_a&&scm_a<mindist)
				{
					lcm_a=mindist;
					lcm_a_i=i;
				}
			}
		}
	}

	rat_polygon_point_manifold_depth(poly_a->world_verts,poly_a->world_axes,poly_a->numverts,poly_b->world_verts[0],&scm_b);
	for (i=1; i<poly_b->numverts; i++)
	{
		vector2 point=poly_b->world_verts[i];
		if (rat_polygon_point_manifold_depth(poly_a->world_verts,poly_a->world_axes,poly_a->numverts,point,&mindist)!=-1)
		{
			if (mindist<0.0)
			{
				if (mindist>scm_b)
				{
					scm_b=mindist;
					scm_b_i=i;
				}
				if (mindist<lcm_b&&scm_b<mindist)
				{
					lcm_b=mindist;
					lcm_b_i=i;
				}
			}
		}
	}

	if (scm_a<scm_b)
	{
		if (scm_a<=0.0)
		{
			con=rat_add_array_contact(*cons);
			con->hull_a=(rat_hull *)poly_a;
			con->hull_b=(rat_hull *)poly_b;
			con->idvala=(unsigned int)poly_a;
			con->idvalb=(unsigned int)poly_a;
			con->point=vector2_multf(vector2_add(poly_a->world_verts[scm_a_i],poly_a->world_verts[lcm_a_i]),0.5);
			con->normal=normal;
			con->penetration=distance;
		}
	}
	else
	{
		if (scm_b<=0.0)
		{
			con=rat_add_array_contact(*cons);
			con->hull_a=(rat_hull *)poly_a;
			con->hull_b=(rat_hull *)poly_b;
			con->idvala=(unsigned int)poly_a;
			con->idvalb=(unsigned int)poly_a;
			con->point=vector2_multf(vector2_add(poly_b->world_verts[scm_b_i],poly_b->world_verts[lcm_b_i]),0.5);
			con->normal=normal;
			con->penetration=distance;
		}
	}
}*/

int rat_coll_poly_poly(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	rat_hull_polygon *poly_a=(rat_hull_polygon *)a;
	rat_hull_polygon *poly_b=(rat_hull_polygon *)b;

	int msa_a_i,msa_b_i;
	rat_real msa_a,msa_b;

	msa_a_i=minimum_separating_axis(poly_b,poly_a->world_axes,poly_a->numverts,&msa_a);
	if (msa_a_i==-1) return 0;

	msa_b_i=minimum_separating_axis(poly_a,poly_b->world_axes,poly_b->numverts,&msa_b);
	if (msa_b_i==-1) return 0;

	if (cons)
	{
		*cons=rat_alloc_contact_array(bal,0);
		if (msa_a>msa_b)
		{
			contact_verts(cons,poly_a,poly_b,poly_a->world_axes[msa_a_i].normal,-1.0,msa_a);
			contact_crossings(cons,poly_a,poly_b,poly_a->world_axes[msa_a_i].normal,-1.0,msa_a);
			//contact_smallest_manifold(cons,poly_a,poly_b,poly_a->world_axes[msa_a_i].normal,-1.0,msa_a);
		}
		else
		{
			contact_verts(cons,poly_a,poly_b,poly_b->world_axes[msa_b_i].normal,1.0,msa_b);
			contact_crossings(cons,poly_a,poly_b,poly_b->world_axes[msa_b_i].normal,1.0,msa_b);
			//contact_smallest_manifold(cons,poly_a,poly_b,poly_b->world_axes[msa_b_i].normal,1.0,msa_b);
		}
	}

	return 1;
}

int rat_coll_poly_particle(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	unsigned int i;

	unsigned int min_interval_i=0;
	rat_real min_interval;

	rat_hull_polygon *polygon=(rat_hull_polygon *)a;
	rat_hull_particle *particle=(rat_hull_particle *)b;

	// get the minimum interval / normal
	min_interval=vector2_dot(polygon->world_axes[0].normal,particle->point)-polygon->world_axes[0].dist;
	for (i=0; i<polygon->numverts; i++)
	{
		rat_real dist=vector2_dot(polygon->world_axes[i].normal,particle->point)-polygon->world_axes[i].dist;
		if (dist>0.0)
			return 0;
		else if (dist>min_interval)
		{
			min_interval=dist;
			min_interval_i=i;
		}
	}

	if (min_interval<0.0)
	{
		if (cons)
		{
			rat_contact *con;
			*cons=rat_alloc_contact_array(bal,1);
			con=rat_indexed_contact(*cons,0);

			con->point=vector2_sub(particle->point,vector2_multf(polygon->world_axes[min_interval_i].normal,min_interval/2.0));
			con->normal=vector2_negative(polygon->world_axes[min_interval_i].normal);
			con->penetration=min_interval;

			con->hull_a=a;
			con->hull_b=b;
			con->idvala=1;
			con->idvalb=2;
		}
		return 1;
	}
	return 0;
}

#define set_coll_query_fn(a,b,f) rat_coll_query_fn_arr[(a)+(b)*rat_num_hull_types]=(f)

void rat_init_collision_array()
{
	if (!rat_is_coll_query_fn_arr_init)
	{
		set_coll_query_fn(rat_hull_type_particle,rat_hull_type_circle,rat_coll_particle_circle);
		set_coll_query_fn(rat_hull_type_particle,rat_hull_type_polygon,rat_coll_particle_poly);
		set_coll_query_fn(rat_hull_type_particle,rat_hull_type_particle,rat_never_collide);

		set_coll_query_fn(rat_hull_type_circle,rat_hull_type_circle,rat_coll_circle_circle);
		set_coll_query_fn(rat_hull_type_circle,rat_hull_type_polygon,rat_coll_circle_poly);
		set_coll_query_fn(rat_hull_type_circle,rat_hull_type_particle,rat_coll_circle_particle);

		set_coll_query_fn(rat_hull_type_polygon,rat_hull_type_circle,rat_coll_poly_circle);
		set_coll_query_fn(rat_hull_type_polygon,rat_hull_type_polygon,rat_coll_poly_poly);
		set_coll_query_fn(rat_hull_type_polygon,rat_hull_type_particle,rat_coll_poly_particle);

		rat_is_coll_query_fn_arr_init=1;
	}
}

int rat_query_collision(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	return rat_coll_query_fn_arr[a->type+b->type*rat_num_hull_types](bal,a,b,cons);
}