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


// these are helpers that add/remove/delete constraints and springs in the world

static void add_constraint(rat_world *world,rat_constraint *constraint)
{
	if (world->numconsts==0)
		world->constraints=(rat_constraint **)rat_alloc(sizeof(rat_constraint *));
	else
		world->constraints=(rat_constraint **)rat_realloc(world->constraints,sizeof(rat_constraint *)*(world->numconsts+1));
	world->constraints[world->numconsts]=constraint;
	world->numconsts++;
}

static void remove_constraint(rat_world *world,rat_constraint *constraint)
{
	register unsigned int i,j;
	if (!constraint) return;

	for (i=0; i<world->numconsts; i++)
	{
		if (world->constraints[i]==constraint)
		{
			for (j=i+1; j<world->numconsts; j++)
				world->constraints[j-1]=world->constraints[j];

			world->numconsts--;
			if (world->numconsts)
				world->constraints=(rat_constraint **)rat_realloc(world->constraints,world->numconsts*sizeof(rat_constraint *));
			else
				rat_dealloc((void *)world->constraints);

			break;
		}
	}
}

static void add_spring(rat_world *world,rat_spring *spring)
{
	if (world->numsprings==0)
		world->springs=(rat_spring **)rat_alloc(sizeof(rat_spring *));
	else
		world->springs=(rat_spring **)rat_realloc(world->springs,sizeof(rat_spring *)*(world->numsprings+1));
	world->springs[world->numsprings]=spring;
	world->numsprings++;
}

static void remove_spring(rat_world *world,rat_spring *spring)
{
	register unsigned int i,j;
	if (!spring) return;

	for (i=0; i<world->numsprings; i++)
	{
		if (world->springs[i]==spring)
		{
			for (j=i+1; j<world->numsprings; j++)
				world->springs[j-1]=world->springs[j];

			world->numsprings--;
			if (world->numsprings)
				world->springs=(rat_spring **)rat_realloc(world->springs,world->numsprings*sizeof(rat_spring *));
			else
				rat_dealloc((void *)world->springs);

			return;
		}
	}
}

void rat_default_body_left_world(rat_world *thisworld,rat_body *thisbody,void *userdata)
{
	// by default we just remove the body from the world.
	// the user of this lib could override it with something
	// that respawns it back inside or something like that.
	rat_world_remove_body(thisworld,thisbody);
}

// do nothing on these.  the user of this lib could
// create a callback that throws sparks on collision,
// or make a sound, for example.
void rat_default_collision_occurs(rat_world *thisworld,rat_arbiter *arbiter,void *userdata)		{}
void rat_default_collision_persists(rat_world *thisworld,rat_arbiter *arbiter,void *userdata)	{}
void rat_default_collision_ending(rat_world *thisworld,rat_arbiter *arbiter,void *userdata)		{}

rat_world *rat_world_create(aabb world_bounds,rat_integrator_type integrator,
	unsigned int collision_iterations,rat_body_manager_type bmtype,void *bmdata)
{
	rat_world *world=(rat_world *)rat_alloc(sizeof(rat_world));
	memset(world,0,sizeof(rat_world));

	world->sal=rat_stack_allocator_create();
	world->bal=rat_block_allocator_create();

	aabb_resolve_order(&world_bounds);
	world->world_bounds=world_bounds;

	world->damping=1.0;
	vector2_set(&(world->gravity),0.0,0.0);

	rat_world_set_solver_iterations(world,collision_iterations);
	world->bias_coef=0.2;

	world->callbacks=(rat_world_callbacks *)rat_alloc(sizeof(rat_world_callbacks));
	memset(world->callbacks,0,sizeof(rat_world_callbacks));
	world->callbacks->body_left_world=rat_default_body_left_world;
	world->callbacks->collision_occurs=rat_default_collision_occurs;
	world->callbacks->collision_persists=rat_default_collision_persists;
	world->callbacks->collision_ending=rat_default_collision_ending;

	switch (integrator)
	{
	case rat_integrator_euler:
		world->body_integrate_position=rat_body_integrate_position_euler;
		break;
	case rat_integrator_rk4:
		world->body_integrate_position=rat_body_integrate_position_rk4;
		break;
	default:
		rat_dealloc((void *)world);
		return NULL;
	}

	switch (bmtype)
	{
	case rat_body_manager_type_array_table:
		rat_init_body_manager_classes();
		// world boundaries do not effect this method because it is
		// brute force only, no spatial indexing at all.
		world->body_manager=rat_body_manager_array_table_create(world);
		break;
	case rat_body_manager_type_quad_tree:
		rat_init_body_manager_classes();
		world->body_manager=rat_body_manager_quad_tree_create(world,world_bounds,*((vector2 *)bmdata));
		break;
	default:
		rat_dealloc((void *)world);
		return NULL;
	}

	world->filters=rat_collision_filter_list_create();

	world->numsprings=world->numconsts=world->numspringsdel=world->numconstsdel=0;
	world->constraints=world->constsdel=NULL;
	world->springs=world->springsdel=NULL;

	rat_init_collision_array();

	world->numislands=0;

	return world;
}

void rat_world_destroy(rat_world *world)
{
	rat_collision_filter_list_destroy(world->filters);
	rat_body_manager_destroy(world->body_manager);
	rat_world_clear_constraints(world);
	rat_world_clear_springs(world);

	rat_dealloc((void *)world->callbacks);

	rat_block_allocator_clear(world->bal);
	rat_block_allocator_destroy(world->bal);
	rat_stack_allocator_destroy(world->sal);

	rat_dealloc((void *)world);
}

void rat_world_set_body_left_world_callback(rat_world *world,void (*body_left_world)(struct rat_world *thisworld,rat_body *thisbody,void *userdata),void *userdata)
{
	world->callbacks->body_left_world=body_left_world;
	world->callbacks->body_left_world_userdata=userdata;
}

void rat_world_set_collision_occurs_callback(rat_world *world,void (*collision_occurs)(struct rat_world *thisworld,rat_arbiter *arbiter,void *userdata),void *userdata)
{
	world->callbacks->collision_occurs=collision_occurs;
	world->callbacks->collision_occurs_userdata=userdata;
}

void rat_world_set_collision_persists_callback(rat_world *world,void (*collision_persists)(struct rat_world *thisworld,rat_arbiter *arbiter,void *userdata),void *userdata)
{
	world->callbacks->collision_persists=collision_persists;
	world->callbacks->collision_persists_userdata=userdata;
}

void rat_world_set_collision_ending_callback(rat_world *world,void (*collision_ending)(struct rat_world *thisworld,rat_arbiter *arbiter,void *userdata),void *userdata)
{
	world->callbacks->collision_ending=collision_ending;
	world->callbacks->collision_ending_userdata=userdata;
}

void rat_world_free_entities(rat_world *world)
{
	register unsigned int i;

	for (i=0; i<world->numconsts; i++)
		rat_constraint_destroy(world->constraints[i]);
	for (i=0; i<world->numsprings; i++)
		rat_spring_destroy(world->springs[i]);
	rat_body_manager_free_bodies(world->body_manager);
	rat_world_clear_constraints(world);
	rat_world_clear_springs(world);
}

void rat_world_awaken_all(rat_world *world)
{
	rat_iterator *iter;
	for (iter=rat_body_manager_all_bodies_iterator(world->body_manager); rat_iterator_finished(iter); rat_iterator_next(iter))
		rat_body_set_is_asleep((rat_body *)rat_iterator_value(iter),0);
	rat_iterator_destroy(iter);
}

void rat_world_add_body(rat_world *world,rat_body *body)
{
	rat_body_manager_add_body(world->body_manager,body);
}

void rat_world_remove_body(rat_world *world,rat_body *body)
{
	rat_body_manager_remove_body(world->body_manager,body);
}

void rat_world_remove_and_destroy_body(rat_world *world,rat_body *body)
{
	rat_body_manager_remove_and_destroy_body(world->body_manager,body);
}

void rat_world_clear_bodies(rat_world *world)
{
	rat_body_manager_clear_bodies(world->body_manager);
}

void rat_world_add_constraint(rat_world *world,rat_constraint *constraint)
{
	add_constraint(world,constraint);
}

void rat_world_remove_constraint(rat_world *world,rat_constraint *constraint)
{
	remove_constraint(world,constraint);
}

void rat_world_remove_and_destroy_constraint(rat_world *world,rat_constraint *constraint)
{
	remove_constraint(world,constraint);

	if (world->numconstsdel==0)
		world->constsdel=(rat_constraint **)rat_alloc(sizeof(rat_constraint *));
	else
		world->constsdel=(rat_constraint **)rat_realloc(world->constsdel,sizeof(rat_constraint *)*(world->numconstsdel+1));
	world->constsdel[world->numconstsdel]=constraint;
	world->numconstsdel++;
}

void rat_world_clear_constraints(rat_world *world)
{
	if (world->numconsts>0)
	{
		rat_dealloc((void *)world->constraints);
		world->numconsts=0;
	}
}

void rat_world_add_spring(rat_world *world,rat_spring *spring)
{
	add_spring(world,spring);
}

void rat_world_remove_spring(rat_world *world,rat_spring *spring)
{
	remove_spring(world,spring);
}

void rat_world_remove_and_destroy_spring(rat_world *world,rat_spring *spring)
{
	remove_spring(world,spring);

	if (world->numspringsdel==0)
		world->springsdel=(rat_spring **)rat_alloc(sizeof(rat_spring *));
	else
		world->springsdel=(rat_spring **)rat_realloc(world->springsdel,sizeof(rat_spring *)*(world->numspringsdel+1));
	world->springsdel[world->numspringsdel]=spring;
	world->numspringsdel++;
}

void rat_world_clear_springs(rat_world *world)
{
	if (world->numsprings>0)
	{
		rat_dealloc((void *)world->springs);
		world->numsprings=0;
	}
}

void rat_world_set_gravity(rat_world *world,vector2 gravity)
{
	world->gravity=gravity;
}

void rat_world_set_damping(rat_world *world,rat_real damping)
{
	world->damping=damping;
}

void rat_world_set_solver_iterations(rat_world *world,unsigned int iterations)
{
	world->inelastic_iterations=iterations;
	world->elastic_iterations=iterations;
	world->elastic_coef=1.0/(rat_real)iterations;
}

unsigned int rat_world_get_solver_iterations(rat_world *world)
{
	return world->inelastic_iterations;
}

vector2 rat_world_get_gravity(rat_world *world)
{
	return world->gravity;
}

rat_real rat_world_get_damping(rat_world *world)
{
	return world->damping;
}

void rat_world_add_collision_filter(rat_world *world,int tag_a,int tag_b,int atroot)
{
	if (atroot)
		rat_collision_filter_list_add_root(world->filters,tag_a,tag_b);
	else
		rat_collision_filter_list_add_last(world->filters,tag_a,tag_b);
}

int rat_world_remove_collision_filter(rat_world *world,int tag_a,int tag_b,int fromlast)
{
	if (fromlast)
		return rat_collision_filter_list_remove_last(world->filters,tag_a,tag_b);
	else
		return rat_collision_filter_list_remove_root(world->filters,tag_a,tag_b);
}

void rat_world_clear_collision_filters(rat_world *world)
{
	rat_collision_filter_list_clear(world->filters);
}

rat_iterator *rat_world_all_bodies_iterator(rat_world *world)
{
	return rat_body_manager_all_bodies_iterator(world->body_manager);
}

rat_iterator *rat_world_static_bodies_iterator(rat_world *world)
{
	return rat_body_manager_static_bodies_iterator(world->body_manager);
}

rat_iterator *rat_world_dynamic_bodies_iterator(rat_world *world)
{
	return rat_body_manager_dynamic_bodies_iterator(world->body_manager);
}

rat_iterator *rat_world_arbiters_iterator(rat_world *world)
{
	return rat_body_manager_arbiters_iterator(world->body_manager);
}

void rat_world_integrate(rat_world *world,rat_real step)
{
	register unsigned int i;
	rat_iterator *iter;

	// we'll just reuse this island, so size for worst case: all on one island.
	rat_island *island=rat_island_create(
		rat_body_manager_count_all_bodies(world->body_manager),
		world->numconsts,world->numsprings,
		rat_body_manager_count_arbiters(world->body_manager),
		world);

	unsigned int stack_size=rat_body_manager_count_all_bodies(world->body_manager);
	rat_body **stack;

	world->numislands=0;

	// kill all island marker flags
	for (iter=rat_body_manager_dynamic_bodies_iterator(world->body_manager); rat_iterator_finished(iter); rat_iterator_next(iter))
	{
		rat_body *thisbody=(rat_body *)rat_iterator_value(iter);
		rat_body_kill_island_marker(thisbody);
	}
	rat_iterator_destroy(iter);
	for (iter=rat_body_manager_arbiters_iterator(world->body_manager); rat_iterator_finished(iter); rat_iterator_next(iter))
	{
		rat_arbiter *thisarbiter=(rat_arbiter *)rat_iterator_value(iter);
		thisarbiter->island_marker=0;
	}
	rat_iterator_destroy(iter);
	for (i=0; i<world->numconsts; i++)
		world->constraints[i]->island_marker=0;
	for (i=0; i<world->numsprings; i++)
		world->springs[i]->island_marker=0;

	// awaken bodies with a one ended spring
	for (i=0; i<world->numsprings; i++)
	{
		if (world->springs[i]->body_b==NULL)
			rat_body_set_is_asleep(world->springs[i]->body_a,0);
	}

	// build and simulate islands
	stack=(rat_body **)rat_stack_alloc(world->sal,sizeof(rat_body *)*stack_size);
	for (iter=rat_body_manager_dynamic_bodies_iterator(world->body_manager); rat_iterator_finished(iter); rat_iterator_next(iter))
	{
		rat_body *seed=(rat_body *)rat_iterator_value(iter);
		unsigned int stackdepth=0;

		if (seed->body_flags&rat_island_flag) continue;	// if already in an island then skip

		rat_island_clear(island);			// clear the island
		stack[stackdepth++]=seed;			// push seed on to stack
		rat_body_set_island_marker(seed);	// set island marker on seed

		if (seed->body_flags&rat_is_asleep_flag) continue;

		// run a deep adjacency search using a stack (depth first search: DFS) through arbiters, constraints, springs.
		while (stackdepth>0)
		{
			rat_body *thisbody=stack[--stackdepth]; // pop next body off stack

			// do not propagate adjacency tree through static bodies
			if (thisbody->mass==0.0) continue;

			// add this body to the island
			rat_island_add_body(island,thisbody);

			// wake up
			rat_body_set_is_asleep(thisbody,0);

			// search adjacent arbiters
			for (i=0; i<thisbody->numarbiters; i++)
			{
				rat_body *otherbody;
				rat_arbiter *thisarbiter=thisbody->arbiters[i];

				// if already on an island then skip
				if (thisarbiter->island_marker) continue;

				// add to island
				rat_island_add_arbiter(island,thisarbiter);
				thisarbiter->island_marker=1;

				// get the other body if it has no island marker
				otherbody=thisarbiter->body_a==thisbody?thisarbiter->body_b:thisarbiter->body_a;
				if (!otherbody) continue;
				if (rat_body_get_island_marker(otherbody)) continue;

				// push it on the stack
				stack[stackdepth++]=otherbody;
				rat_body_set_island_marker(otherbody);
			}

			// search adjacent constraints
			for (i=0; i<thisbody->numconsts; i++)
			{
				rat_body *otherbody;
				rat_constraint *thisconstraint=thisbody->constraints[i];

				// if already on an island then skip
				if (thisconstraint->island_marker) continue;

				// add to island
				rat_island_add_constraint(island,thisconstraint);
				thisconstraint->island_marker=1;

				// get the other body if it has no island marker
				otherbody=thisconstraint->body_a==thisbody?thisconstraint->body_b:thisconstraint->body_a;
				if (!otherbody) continue;
				if (rat_body_get_island_marker(otherbody)) continue;

				// push it on the stack
				stack[stackdepth++]=otherbody;
				rat_body_set_island_marker(otherbody);
			}

			// search adjacent springs
			for (i=0; i<thisbody->numsprings; i++)
			{
				rat_body *otherbody;
				rat_spring *thisspring=thisbody->springs[i];

				// if already on an island then skip
				if (thisspring->island_marker) continue;

				// add to island
				rat_island_add_spring(island,thisspring);
				thisspring->island_marker=1;

				// get the other body if it has no island marker
				otherbody=thisspring->body_a==thisbody?thisspring->body_b:thisspring->body_a;
				if (!otherbody) continue;
				if (rat_body_get_island_marker(otherbody)) continue;

				// push it on the stack
				stack[stackdepth++]=otherbody;
				rat_body_set_island_marker(otherbody);
			}
		}
		world->numislands++;

		// solve the island
		rat_island_integrate(island,step);
	}
	rat_iterator_destroy(iter);

	rat_stack_dealloc(world->sal,stack);
	rat_island_destroy(island);
}

void rat_world_update(rat_world *world,rat_real timestep,int clean_blocks)
{
	register unsigned int i,j;
	rat_real inv_timestep;
	rat_iterator *iter;
	rat_body **world_leavers=NULL;
	if (!timestep) return;

	// integrate position
	for (iter=rat_body_manager_dynamic_bodies_iterator(world->body_manager); rat_iterator_finished(iter); rat_iterator_next(iter))
	{
		rat_body *thisbody=(rat_body *)rat_iterator_value(iter);
		if (!rat_body_get_is_asleep(thisbody))
			world->body_integrate_position(thisbody,timestep);
	}
	rat_iterator_destroy(iter);

	// cache bounding boxes and world transforms
	for (iter=rat_body_manager_all_bodies_iterator(world->body_manager); rat_iterator_finished(iter); rat_iterator_next(iter))
	{
		rat_body *thisbody=(rat_body *)rat_iterator_value(iter);
		if (!rat_body_get_is_asleep(thisbody))
			rat_hull_update_cache(thisbody->hull,thisbody->mass==0.0);
	}
	rat_iterator_destroy(iter);

	// collide
	rat_body_manager_collide_bodies(world->body_manager);

	// integrate the world
	rat_world_integrate(world,timestep);

	// check if we left the world
	for (iter=rat_body_manager_dynamic_bodies_iterator(world->body_manager); rat_iterator_finished(iter); rat_iterator_next(iter))
	{
		rat_body *thisbod=(rat_body *)rat_iterator_value(iter);
		if (!aabb_is_aabb_overlapping(thisbod->hull->cached_bb,world->world_bounds))
			world->callbacks->body_left_world(world,thisbod,world->callbacks->body_left_world_userdata);
	}
	rat_iterator_destroy(iter);

	// constraints to be deleted
	if (world->numconstsdel)
	{
		for (i=0; i<world->numconstsdel; i++)
			rat_constraint_destroy(world->constsdel[i]);
		world->numconstsdel=0;
		rat_dealloc((void *)world->constsdel);
	}

	// springs to be deleted
	if (world->numspringsdel)
	{
		for (i=0; i<world->numspringsdel; i++)
			rat_spring_destroy(world->springsdel[i]);
		world->numspringsdel=0;
		rat_dealloc((void *)world->springsdel);
	}

	// clean the contact block allocator's recycler
	if (rat_block_allocator_blocks_alloced(world->bal))
	{
		fprintf(stderr,"%u recylable blocks still allocated after end of world update!!!\n",
			rat_block_allocator_blocks_alloced(world->bal));
		fflush(stderr);
	}
	if (clean_blocks) rat_block_allocator_clear(world->bal);
}