/*
 * 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 "body_manager_array_table.h"
#include "world.h"

static void add_arbiter(rat_body_manager_array_table *mgr,rat_arbiter *arbiter)
{
	register unsigned int maxarbs=mgr->maxarbs>(mgr->numarbs+1)?mgr->maxarbs:(mgr->numarbs+1);
	if (mgr->maxarbs==0)
	{
		mgr->maxarbs=1;
		mgr->arbiters=(rat_arbiter **)rat_alloc(sizeof(rat_arbiter *));
	}
	else if (maxarbs>mgr->maxarbs)
	{
		mgr->maxarbs=maxarbs;
		mgr->arbiters=(rat_arbiter **)rat_realloc(mgr->arbiters,sizeof(rat_arbiter *)*mgr->maxarbs);
	}

	mgr->arbiters[mgr->numarbs++]=arbiter;
}

static void remove_arbiter(rat_body_manager_array_table *mgr,unsigned int i)
{
	register unsigned int j;

	rat_arbiter_destroy(mgr->arbiters[i]);
	mgr->numarbs--;

	if (mgr->numarbs>0)
	{
		for (j=i+1; j<mgr->numarbs+1; j++)
			mgr->arbiters[j-1]=mgr->arbiters[j];
	}
}

static void remove_arbiter_dat(rat_body_manager_array_table *mgr,rat_arbiter *arb)
{
	register unsigned int i;

	for (i=0; i<mgr->numarbs; i++)
	{
		if (mgr->arbiters[i]==arb)
		{
			remove_arbiter(mgr,i);
			break;
		}
	}
}

rat_body_manager *rat_body_manager_array_table_create(rat_world *world_ref)
{
	rat_body_manager_array_table *table=(rat_body_manager_array_table *)rat_alloc(sizeof(rat_body_manager_array_table));

	table->type=rat_body_manager_type_array_table;
	table->clas=&rat_array_table_class;
	table->world_ref=world_ref;

	table->numbods=table->numbodsrmv=
		table->numbodsrmvdel=
		table->numstatic=table->numdynamic=
		table->numarbs=
		table->maxarbs=0;

	table->bodies=NULL;
	table->static_bodies=NULL;
	table->dynamic_bodies=NULL;
	table->arbiters=NULL;

	return (rat_body_manager *)table;
}

void rat_body_manager_array_table_destroy(rat_body_manager *table)
{
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	if (mgr->arbiters&&mgr->maxarbs)
	{
		register unsigned int i;
		for (i=0; i<mgr->numarbs; i++)
			rat_arbiter_destroy(mgr->arbiters[i]);
		rat_dealloc((void *)mgr->arbiters);
	}

	if (mgr->numbods) rat_dealloc((void *)mgr->bodies);
	if (mgr->numstatic) rat_dealloc((void *)mgr->static_bodies);
	if (mgr->numdynamic) rat_dealloc((void *)mgr->dynamic_bodies);
	if (mgr->numbodsrmv) rat_dealloc((void *)mgr->bodiestoremove);
	if (mgr->numbodsrmvdel) rat_dealloc((void *)mgr->bodiestodel);

	rat_dealloc((void *)mgr);
}

void rat_body_manager_array_table_add_body(rat_body_manager *table,rat_body *body)
{
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	if (mgr->numbods==0)
		mgr->bodies=(rat_body **)rat_alloc(sizeof(rat_body *));
	else
		mgr->bodies=(rat_body **)rat_realloc(mgr->bodies,sizeof(rat_body *)*(mgr->numbods+1));
	mgr->bodies[mgr->numbods]=body;
	mgr->numbods++;

	if (body->mass==0.0)
	{
		if (mgr->numstatic==0)
			mgr->static_bodies=(rat_body **)rat_alloc(sizeof(rat_body *));
		else
			mgr->static_bodies=(rat_body **)rat_realloc(mgr->static_bodies,sizeof(rat_body *)*(mgr->numstatic+1));
		mgr->static_bodies[mgr->numstatic]=body;
		mgr->numstatic++;
	}
	else
	{
		if (mgr->numdynamic==0)
			mgr->dynamic_bodies=(rat_body **)rat_alloc(sizeof(rat_body *));
		else
			mgr->dynamic_bodies=(rat_body **)rat_realloc(mgr->dynamic_bodies,sizeof(rat_body *)*(mgr->numdynamic+1));
		mgr->dynamic_bodies[mgr->numdynamic]=body;
		mgr->numdynamic++;
	}
}


void rat_body_manager_array_table_remove_body(rat_body_manager *table,rat_body *body)
{
	register unsigned int i;
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	for (i=0; i<mgr->numbodsrmv; i++)
	{
		if (mgr->bodiestoremove[i]==body) return;
	}

	if (mgr->numbodsrmv==0)
		mgr->bodiestoremove=(rat_body **)rat_alloc(sizeof(rat_body *));
	else
		mgr->bodiestoremove=(rat_body **)rat_realloc(mgr->bodiestoremove,sizeof(rat_body *)*(mgr->numbodsrmv+1));
	mgr->bodiestoremove[mgr->numbodsrmv]=body;
	mgr->numbodsrmv++;

	// delete any associated arbiters
	for (i=0; i<mgr->numarbs; i++)
	{
		if (mgr->arbiters[i]->body_a==body||mgr->arbiters[i]->body_b==body)
		{
			remove_arbiter(mgr,i);
			i--;
		}
	}
}

void rat_body_manager_array_table_remove_and_destroy_body(rat_body_manager *table,rat_body *body)
{
	register unsigned int i;
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	for (i=0; i<mgr->numbodsrmvdel; i++)
	{
		if (mgr->bodiestodel[i]==body) return;
	}

	if (mgr->numbodsrmvdel==0)
		mgr->bodiestodel=(rat_body **)rat_alloc(sizeof(rat_body *));
	else
		mgr->bodiestodel=(rat_body **)rat_realloc(mgr->bodiestodel,sizeof(rat_body *)*(mgr->numbodsrmvdel+1));
	mgr->bodiestodel[mgr->numbodsrmvdel]=body;
	mgr->numbodsrmvdel++;

	// delete any associated arbiters
	for (i=0; i<mgr->numarbs; i++)
	{
		if (mgr->arbiters[i]->body_a==body||mgr->arbiters[i]->body_b==body)
		{
			remove_arbiter(mgr,i);
			i--;
		}
	}
}

void really_remove_body(rat_body_manager *table,rat_body *body)
{
	register unsigned int i,j;
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	// dereference from associated springs
	for (i=0; i<body->numsprings; i++)
	{
		if (body->springs[i]->body_a==body)
			body->springs[i]->body_a=NULL;
		else
			body->springs[i]->body_b=NULL;
	}

	// dereference from associated constraints
	for (i=0; i<body->numconsts; i++)
	{
		if (body->constraints[i]->body_a==body)
			body->constraints[i]->body_a=NULL;
		else
			body->constraints[i]->body_b=NULL;
	}

	// delete the body from it's lists
	if (body->mass==0.0)
	{
		for (i=0; i<mgr->numstatic; i++)
		{
			if (mgr->static_bodies[i]==body)
			{
				for (j=i+1; j<mgr->numstatic; j++)
					mgr->static_bodies[j-1]=mgr->static_bodies[j];

				mgr->numstatic--;
				if (mgr->numstatic)
					mgr->static_bodies=(rat_body **)rat_realloc(mgr->static_bodies,mgr->numstatic*sizeof(rat_body *));
				else
					rat_dealloc((void *)mgr->static_bodies);

				break;
			}
		}
	}
	else
	{
		for (i=0; i<mgr->numdynamic; i++)
		{
			if (mgr->dynamic_bodies[i]==body)
			{
				for (j=i+1; j<mgr->numdynamic; j++)
					mgr->dynamic_bodies[j-1]=mgr->dynamic_bodies[j];

				mgr->numdynamic--;
				if (mgr->numdynamic)
					mgr->dynamic_bodies=(rat_body **)rat_realloc(mgr->dynamic_bodies,mgr->numdynamic*sizeof(rat_body *));
				else
					rat_dealloc((void *)mgr->dynamic_bodies);

				break;
			}
		}
	}
	for (i=0; i<mgr->numbods; i++)
	{
		if (mgr->bodies[i]==body)
		{
			for (j=i+1; j<mgr->numbods; j++)
				mgr->bodies[j-1]=mgr->bodies[j];

			mgr->numbods--;
			if (mgr->numbods)
				mgr->bodies=(rat_body **)rat_realloc(mgr->bodies,mgr->numbods*sizeof(rat_body *));
			else
				rat_dealloc((void *)mgr->bodies);

			break;
		}
	}
}

void rat_body_manager_array_table_clear_bodies(rat_body_manager *table)
{
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	if (mgr->numbods)
	{
		rat_dealloc((void *)mgr->bodies);
		if (mgr->numstatic) rat_dealloc((void *)mgr->static_bodies);
		if (mgr->numdynamic) rat_dealloc((void *)mgr->dynamic_bodies);
	}
	mgr->numbods=mgr->numstatic=mgr->numdynamic=0;
}

void rat_body_manager_array_table_free_bodies(rat_body_manager *table)
{
	register unsigned int i;
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	if (mgr->arbiters&&mgr->maxarbs)
	{
		register unsigned int i;
		for (i=0; i<mgr->numarbs; i++)
			rat_arbiter_destroy(mgr->arbiters[i]);
		mgr->numarbs=0;
	}

	if (mgr->numstatic) rat_dealloc((void *)mgr->static_bodies);
	if (mgr->numdynamic) rat_dealloc((void *)mgr->dynamic_bodies);
	if (mgr->numbodsrmv) rat_dealloc((void *)mgr->bodiestoremove);
	if (mgr->numbodsrmvdel) rat_dealloc((void *)mgr->bodiestodel);

	if (mgr->numbods>0)
	{
		for (i=0; i<mgr->numbods; i++)
			rat_body_destroy(mgr->bodies[i]);

		rat_dealloc((void *)mgr->bodies);
	}
	mgr->numbods=mgr->numstatic=mgr->numdynamic=mgr->numarbs=mgr->numbodsrmv=mgr->numbodsrmvdel=0;
}

static int is_colliding_helper(rat_world *world,rat_body *a,rat_body *b,rat_contact_array **cons)
{
	register unsigned int i;
	if (!aabb_is_aabb_overlapping(a->hull->cached_bb,b->hull->cached_bb)) return 0;
	if (rat_collision_filter_list_hit(world->filters,a->filtertag,b->filtertag)) return 0;
	if (rat_body_is_adjacency_filter_enabled(a))
	{
		for (i=0; i<a->numconsts; i++)
		{
			rat_body *other=a->constraints[i]->body_a==a?a->constraints[i]->body_b:a->constraints[i]->body_a;
			if (other==b) return 0;
		}
		for (i=0; i<a->numsprings; i++)
		{
			rat_body *other=a->springs[i]->body_a==a?a->springs[i]->body_b:a->springs[i]->body_a;
			if (other==b) return 0;
		}
	}
	else if (rat_body_is_adjacency_filter_enabled(b))
	{
		for (i=0; i<b->numconsts; i++)
		{
			rat_body *other=b->constraints[i]->body_a==b?b->constraints[i]->body_b:b->constraints[i]->body_a;
			if (other==a) return 0;
		}
		for (i=0; i<b->numsprings; i++)
		{
			rat_body *other=b->springs[i]->body_a==b?b->springs[i]->body_b:b->springs[i]->body_a;
			if (other==a) return 0;
		}
	}
	return rat_query_collision(world->bal,a->hull,b->hull,cons);
}

void rat_body_manager_array_table_build_spatial_index(rat_body_manager *table)
{
	// nothing to do here because there is no spatial
	// indexing; this is just the brute force collider
}

void rat_body_manager_array_table_collide_bodies(rat_body_manager *table)
{
	register unsigned int i,j,k;
	register int skip;
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	// remove all bodies with requested removal
	if (mgr->numbodsrmv)
	{
		for (i=0; i<mgr->numbodsrmv; i++)
		{
			skip=0;
			for (j=0; j<mgr->numbodsrmvdel; j++)
			{
				if (mgr->bodiestodel[j]==mgr->bodiestoremove[i])
				{
					skip=1;
					break;
				}
			}
			if (!skip)
				really_remove_body((rat_body_manager *)mgr,mgr->bodiestoremove[i]);

		}
		rat_dealloc((void *)mgr->bodiestoremove);
		mgr->numbodsrmv=0;
	}

	// remove and delete all bodies with requested deletion
	if (mgr->numbodsrmvdel)
	{
		for (i=0; i<mgr->numbodsrmvdel; i++)
		{
			rat_body *bodtodel=mgr->bodiestodel[i];
			really_remove_body((rat_body_manager *)mgr,bodtodel);
			rat_body_destroy(bodtodel);
		}
		rat_dealloc((void *)mgr->bodiestodel);
		mgr->numbodsrmvdel=0;
	}

	// collide static to dynamic
	for (i=0; i<mgr->numstatic; i++)
	{
		for (j=0; j<mgr->numdynamic; j++)
		{
			rat_contact_array *cons=NULL;
			int collided=is_colliding_helper(mgr->world_ref,mgr->static_bodies[i],mgr->dynamic_bodies[j],&cons);

			// bingo,this is a collision!
			if (collided)
			{
				// check if this is a persistent collision
				unsigned int is_persistent=0;
				for (k=0; k<mgr->numarbs; k++)
				{
					if (mgr->arbiters[k]->body_a==mgr->static_bodies[i]&&mgr->arbiters[k]->body_b==mgr->dynamic_bodies[j])
					{
						// if so, we merge this contact information with the old
						rat_arbiter_update_contacts(mgr->arbiters[k],cons);
						mgr->world_ref->callbacks->collision_persists(mgr->world_ref,mgr->arbiters[k],mgr->world_ref->callbacks->collision_persists_userdata);
						is_persistent=1;
						break;
					}
					else if (mgr->arbiters[k]->body_a==mgr->dynamic_bodies[j]&&mgr->arbiters[k]->body_b==mgr->static_bodies[i])
					{
						// we must reverse the hull order in order to update the contact info right
						rat_free_contact_array(cons);
						is_colliding_helper(mgr->world_ref,mgr->dynamic_bodies[j],mgr->dynamic_bodies[i],&cons);
						rat_arbiter_update_contacts(mgr->arbiters[k],cons);
						mgr->world_ref->callbacks->collision_persists(mgr->world_ref,mgr->arbiters[k],mgr->world_ref->callbacks->collision_persists_userdata);
						is_persistent=1;
						break;
					}
				}
				// if not,we create a new arbiter
				if (!is_persistent)
				{
					rat_arbiter *newarbiter=rat_arbiter_create(mgr->world_ref->bal,mgr->static_bodies[i],mgr->dynamic_bodies[j],cons);
					add_arbiter(mgr,newarbiter);
					mgr->world_ref->callbacks->collision_occurs(mgr->world_ref,newarbiter,mgr->world_ref->callbacks->collision_occurs_userdata);
				}

				rat_free_contact_array(cons);
			}
			else // not colliding.  if exists, destroy old arbiter
			{
				for (k=0; k<mgr->numarbs; k++)
				{
					 if ((mgr->arbiters[k]->body_a==mgr->static_bodies[i]&&mgr->arbiters[k]->body_b==mgr->dynamic_bodies[j])
						||(mgr->arbiters[k]->body_a==mgr->dynamic_bodies[j]&&mgr->arbiters[k]->body_b==mgr->static_bodies[i]))
					{
						mgr->world_ref->callbacks->collision_ending(mgr->world_ref,mgr->arbiters[k],mgr->world_ref->callbacks->collision_ending_userdata);
						remove_arbiter(mgr,k);
						break;
					}
				}
			}
		}
	}

	// collide dynamic to dynamic
	for (i=0; i<mgr->numdynamic; i++)
	{
		for (j=i+1; j<mgr->numdynamic; j++)
		{
			rat_contact_array *cons=NULL;
			int collided=is_colliding_helper(mgr->world_ref,mgr->dynamic_bodies[i],mgr->dynamic_bodies[j],&cons);

			// bingo,this is a collision!
			if (collided)
			{
				// check if this is a persistent collision
				unsigned int is_persistent=0;
				for (k=0; k<mgr->numarbs; k++)
				{
					if (mgr->arbiters[k]->body_a==mgr->dynamic_bodies[i]&&mgr->arbiters[k]->body_b==mgr->dynamic_bodies[j])
					{
						// if so, we merge this contact information with the old
						rat_arbiter_update_contacts(mgr->arbiters[k],cons);
						mgr->world_ref->callbacks->collision_persists(mgr->world_ref,mgr->arbiters[k],mgr->world_ref->callbacks->collision_persists_userdata);
						is_persistent=1;
						break;
					}
					else if (mgr->arbiters[k]->body_a==mgr->dynamic_bodies[j]&&mgr->arbiters[k]->body_b==mgr->dynamic_bodies[i])
					{
						// we must reverse the hull order in order to update the contact info right
						rat_free_contact_array(cons);
						is_colliding_helper(mgr->world_ref,mgr->dynamic_bodies[j],mgr->dynamic_bodies[i],&cons);
						rat_arbiter_update_contacts(mgr->arbiters[k],cons);
						mgr->world_ref->callbacks->collision_persists(mgr->world_ref,mgr->arbiters[k],mgr->world_ref->callbacks->collision_persists_userdata);
						is_persistent=1;
						break;
					}
				}
				// if not,we create a new arbiter
				if (!is_persistent)
				{
					rat_arbiter *newarbiter=rat_arbiter_create(mgr->world_ref->bal,mgr->dynamic_bodies[i],mgr->dynamic_bodies[j],cons);
					add_arbiter(mgr,newarbiter);
					mgr->world_ref->callbacks->collision_occurs(mgr->world_ref,newarbiter,mgr->world_ref->callbacks->collision_occurs_userdata);
				}

				rat_free_contact_array(cons);
			}
			else // not colliding.  if exists, destroy old arbiter
			{
				for (k=0; k<mgr->numarbs; k++)
				{
					 if ((mgr->arbiters[k]->body_a==mgr->dynamic_bodies[i]&&mgr->arbiters[k]->body_b==mgr->dynamic_bodies[j])
						||(mgr->arbiters[k]->body_a==mgr->dynamic_bodies[j]&&mgr->arbiters[k]->body_b==mgr->dynamic_bodies[i]))
					{
						mgr->world_ref->callbacks->collision_ending(mgr->world_ref,mgr->arbiters[k],mgr->world_ref->callbacks->collision_ending_userdata);
						remove_arbiter(mgr,k);
						break;
					}
				}
			}
		}
	}
}

unsigned int rat_body_manager_array_table_count_all_bodies(rat_body_manager *table)
{
	return ((rat_body_manager_array_table *)table)->numbods;
}

unsigned int rat_body_manager_array_table_count_static_bodies(rat_body_manager *table)
{
	return ((rat_body_manager_array_table *)table)->numstatic;
}

unsigned int rat_body_manager_array_table_count_dynamic_bodies(rat_body_manager *table)
{
	return ((rat_body_manager_array_table *)table)->numdynamic;
}

unsigned int rat_body_manager_array_table_count_arbiters(rat_body_manager *table)
{
	return ((rat_body_manager_array_table *)table)->numarbs;
}

rat_iterator *rat_body_manager_array_table_all_bodies_iterator(rat_body_manager *table)
{
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	rat_body_array_iterator *arr=(rat_body_array_iterator *)rat_alloc(sizeof(rat_body_array_iterator));
	arr->clas=(rat_iterator_class *)rat_alloc(sizeof(rat_iterator_class));
	arr->clas->next=rat_body_array_next;
	arr->clas->finished=rat_body_array_finished;
	arr->clas->value=rat_body_array_value;
	arr->clas->destroy=rat_body_array_destroy;
	arr->i=0;
	arr->bodies=mgr->bodies;
	arr->numbods=mgr->numbods;

	return (rat_iterator *)arr;
}

rat_iterator *rat_body_manager_array_table_static_bodies_iterator(rat_body_manager *table)
{
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	rat_body_array_iterator *arr=(rat_body_array_iterator *)rat_alloc(sizeof(rat_body_array_iterator));
	arr->clas=(rat_iterator_class *)rat_alloc(sizeof(rat_iterator_class));
	arr->clas->next=rat_body_array_next;
	arr->clas->finished=rat_body_array_finished;
	arr->clas->value=rat_body_array_value;
	arr->clas->destroy=rat_body_array_destroy;
	arr->i=0;
	arr->bodies=mgr->static_bodies;
	arr->numbods=mgr->numstatic;

	return (rat_iterator *)arr;
}

rat_iterator *rat_body_manager_array_table_dynamic_bodies_iterator(rat_body_manager *table)
{
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	rat_body_array_iterator *arr=(rat_body_array_iterator *)rat_alloc(sizeof(rat_body_array_iterator));
	arr->clas=(rat_iterator_class *)rat_alloc(sizeof(rat_iterator_class));
	arr->clas->next=rat_body_array_next;
	arr->clas->finished=rat_body_array_finished;
	arr->clas->value=rat_body_array_value;
	arr->clas->destroy=rat_body_array_destroy;
	arr->i=0;
	arr->bodies=mgr->dynamic_bodies;
	arr->numbods=mgr->numdynamic;

	return (rat_iterator *)arr;
}

rat_iterator *rat_body_manager_array_table_arbiters_iterator(rat_body_manager *table)
{
	rat_body_manager_array_table *mgr=(rat_body_manager_array_table *)table;

	rat_arbiter_array_iterator *arr=(rat_arbiter_array_iterator *)rat_alloc(sizeof(rat_arbiter_array_iterator));
	arr->clas=(rat_iterator_class *)rat_alloc(sizeof(rat_iterator_class));
	arr->clas->next=rat_arbiter_array_next;
	arr->clas->finished=rat_arbiter_array_finished;
	arr->clas->value=rat_arbiter_array_value;
	arr->clas->destroy=rat_arbiter_array_destroy;
	arr->i=0;
	arr->arbiters=mgr->arbiters;
	arr->numarbs=mgr->numarbs;

	return (rat_iterator *)arr;
}