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

static void add_arbiter(rat_body_manager_quad_tree *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_quad_tree *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_quad_tree *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;
		}
	}
}

static void really_remove_body(rat_body_manager *table,rat_body *body)
{
	register unsigned int i,j;
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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;
	}

	rat_body_destroy_springs(body,mgr->world_ref);
	rat_body_destroy_constraints(body,mgr->world_ref);

	// 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;
		}
	}
}

rat_body_manager *rat_body_manager_quad_tree_create(rat_world *world_ref,aabb worldsize,vector2 approxcell)
{
	register unsigned int numcells_x,numcells_y;
	register vector2 cellsize=vector2_sub(worldsize.b,worldsize.a);
	rat_body_manager_quad_tree *table=(rat_body_manager_quad_tree *)rat_alloc(sizeof(rat_body_manager_quad_tree));

	table->type=rat_body_manager_type_quad_tree;
	table->clas=&rat_quad_tree_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;

	numcells_x=(unsigned int)(cellsize.x/approxcell.x);
	numcells_y=(unsigned int)(cellsize.y/approxcell.y);

	cellsize.x/=(rat_real)numcells_x;
	cellsize.y/=(rat_real)numcells_y;

	table->quadtreematrix=rat_quad_tree_matrix_create(worldsize.a,cellsize,numcells_x,numcells_y);

	return (rat_body_manager *)table;
}

void rat_body_manager_quad_tree_destroy(rat_body_manager *table)
{
	register unsigned int i;

	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)table;
	rat_quad_tree_matrix_destroy(mgr->quadtreematrix);

	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_quad_tree_add_body(rat_body_manager *table,rat_body *body)
{
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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_quad_tree_remove_body(rat_body_manager *table,rat_body *body)
{
	register unsigned int i;
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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_quad_tree_remove_and_destroy_body(rat_body_manager *table,rat_body *body)
{
	register unsigned int i,j;
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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 rat_body_manager_quad_tree_clear_bodies(rat_body_manager *table)
{
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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);
		if (mgr->numbodsrmv) rat_dealloc((void *)mgr->bodiestoremove);
		if (mgr->numbodsrmvdel) rat_dealloc((void *)mgr->bodiestodel);
	}
	mgr->numbods=mgr->numstatic=mgr->numdynamic=0;
}

void rat_body_manager_quad_tree_free_bodies(rat_body_manager *table)
{
	register unsigned int i;
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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;
}

void rat_body_manager_quad_tree_build_spatial_index(rat_body_manager *table)
{
	register unsigned int i,j;
	register int skip;
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)table;

	rat_quad_tree_matrix_clear(mgr->quadtreematrix);
	rat_quad_tree_matrix_build(mgr->quadtreematrix,mgr->bodies,mgr->numbods);
}

void rat_body_manager_quad_tree_collide_bodies(rat_body_manager *table)
{
	register unsigned int i,j;
	register int skip;
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)table;

	// check each arbiter to make sure every collided body is still touching
	for (i=0; i<mgr->numarbs; i++)
	{
		if (!rat_query_collision(mgr->world_ref->bal,mgr->arbiters[i]->body_a->hull,mgr->arbiters[i]->body_b->hull,NULL))
		{
			remove_arbiter(mgr,i);
			i--;
		}
	}

	// 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;
	}

	rat_body_manager_quad_tree_build_spatial_index(table);

	// iterate quad tree with collision function
	rat_quad_tree_matrix_eachin(mgr->quadtreematrix,&rat_quad_tree_collide,(void *)mgr);
}

unsigned int rat_body_manager_quad_tree_count_all_bodies(rat_body_manager *table)
{
	return ((rat_body_manager_quad_tree *)table)->numbods;
}

unsigned int rat_body_manager_quad_tree_count_static_bodies(rat_body_manager *table)
{
	return ((rat_body_manager_quad_tree *)table)->numstatic;
}

unsigned int rat_body_manager_quad_tree_count_dynamic_bodies(rat_body_manager *table)
{
	return ((rat_body_manager_quad_tree *)table)->numdynamic;
}

unsigned int rat_body_manager_quad_tree_count_arbiters(rat_body_manager *table)
{
	return ((rat_body_manager_quad_tree *)table)->numarbs;
}

rat_iterator *rat_body_manager_quad_tree_all_bodies_iterator(rat_body_manager *table)
{
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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_quad_tree_static_bodies_iterator(rat_body_manager *table)
{
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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_quad_tree_dynamic_bodies_iterator(rat_body_manager *table)
{
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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_quad_tree_arbiters_iterator(rat_body_manager *table)
{
	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)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;
}

static int is_colliding_helper(rat_block_allocator *bal,rat_hull *a,rat_hull *b,rat_contact_array **cons)
{
	if (!aabb_is_aabb_overlapping(a->cached_bb,b->cached_bb)) return 0;
	return rat_query_collision(bal,a,b,cons);
}

void rat_quad_tree_collide(rat_body *a,rat_body *b,void *data)
{
	register unsigned int i;

	rat_body_manager_quad_tree *mgr=(rat_body_manager_quad_tree *)data;

	rat_contact_array *cons=NULL;
	int collided;
	if (a->mass==0.0&&b->mass==0.0) return;
	if (rat_collision_filter_list_hit(mgr->world_ref->filters,a->filtertag,b->filtertag)) return;
	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;
		}
		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;
		}
	}
	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;
		}
		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;
		}
	}
	collided=is_colliding_helper(mgr->world_ref->bal,a->hull,b->hull,&cons);

	// bingo, this is a collision!
	if (collided)
	{
		// check if this is a persistent collision
		int is_persistent=0;
		for (i=0; i<mgr->numarbs; i++)
		{
			if (mgr->arbiters[i]->body_a==a&&mgr->arbiters[i]->body_b==b)
			{
				// if so,we merge this contact information with the old
				if (!mgr->arbiters[i]->stamped) // make sure there's no current stamp
				{
					rat_arbiter_update_contacts(mgr->arbiters[i],cons);
					mgr->world_ref->callbacks->collision_persists(mgr->world_ref,mgr->arbiters[i],mgr->world_ref->callbacks->collision_persists_userdata);
				}
				is_persistent=1;
				break;
			}
			else if (mgr->arbiters[i]->body_a==b&&mgr->arbiters[i]->body_b==a)
			{
				// if so,we merge this contact information with the old
				if (!mgr->arbiters[i]->stamped) // make sure there's no current stamp
				{
					// 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->bal,b->hull,a->hull,&cons);
					rat_arbiter_update_contacts(mgr->arbiters[i],cons);
					mgr->world_ref->callbacks->collision_persists(mgr->world_ref,mgr->arbiters[i],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,a,b,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 (i=0; i<mgr->numarbs; i++)
		{
			if ((mgr->arbiters[i]->body_a==a&&mgr->arbiters[i]->body_b==b)
				||(mgr->arbiters[i]->body_a==b&&mgr->arbiters[i]->body_b==a))
			{
				// make sure there's no current stamp
				//if (!mgr->arbiters[i]->stamped)
				//{
				printf("wtf\n");
					mgr->world_ref->callbacks->collision_ending(mgr->world_ref,mgr->arbiters[i],mgr->world_ref->callbacks->collision_ending_userdata);
					remove_arbiter(mgr,i);
				//}
				break;
			}
		}
	}
}