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

rat_island *rat_island_create(unsigned int maxbods,unsigned int maxconsts,unsigned int maxsprings,unsigned int maxarbs,rat_world *world_ref)
{
	rat_island *island=(rat_island *)rat_stack_alloc(world_ref->sal,sizeof(rat_island));
	memset(island,0,sizeof(rat_island));

	island->world_ref=world_ref;
	island->sal=world_ref->sal;

	island->bodies=(rat_body **)rat_stack_alloc(island->sal,sizeof(rat_body *)*maxbods);
	island->constraints=(rat_constraint **)rat_stack_alloc(island->sal,sizeof(rat_constraint *)*maxconsts);
	island->springs=(rat_spring **)rat_stack_alloc(island->sal,sizeof(rat_spring *)*maxsprings);
	island->arbiters=(rat_arbiter **)rat_stack_alloc(island->sal,sizeof(rat_arbiter *)*maxarbs);

	return island;
}

void rat_island_destroy(rat_island *island)
{
	rat_stack_dealloc(island->sal,(void *)island->arbiters);
	rat_stack_dealloc(island->sal,(void *)island->springs);
	rat_stack_dealloc(island->sal,(void *)island->constraints);
	rat_stack_dealloc(island->sal,(void *)island->bodies);

	rat_stack_dealloc(island->sal,(void *)island);
}

void rat_island_add_body(rat_island *island,rat_body *body)
{
	island->bodies[island->numbods++]=body;
}

void rat_island_add_constraint(rat_island *island,rat_constraint *constraint)
{
	island->constraints[island->numconsts++]=constraint;
}

void rat_island_add_spring(rat_island *island,rat_spring *spring)
{
	island->springs[island->numsprings++]=spring;
}

void rat_island_add_arbiter(rat_island *island,rat_arbiter *arbiter)
{
	island->arbiters[island->numarbs++]=arbiter;
}

void rat_island_clear(rat_island *island)
{
	island->numbods=0;
	island->numconsts=0;
	island->numsprings=0;
	island->numarbs=0;
}

void rat_island_integrate(rat_island *island,rat_real step)
{
	register unsigned int i,j;
	const rat_real linear_tolerance_sq=rat_linear_sleep_tolerance*rat_linear_sleep_tolerance;
	const rat_real angular_tolerance_sq=rat_angular_sleep_tolerance*rat_angular_sleep_tolerance;
	rat_real min_sleep_time=FLT_MAX;
	rat_real inv_step=(inv_step=1.0/step)>1.0?1.0:inv_step;

	// prime arbiters
	for (i=0; i<island->numarbs; i++)
		rat_arbiter_prime(island->arbiters[i],island->world_ref->bias_coef*(1.0/step));

	// prime constraints
	for (i=0; i<island->numconsts; i++)
		rat_constraint_prime(island->constraints[i],island->world_ref->bias_coef*inv_step);

	// solve springs
	for (j=0; j<island->numsprings; j++)
		rat_spring_integrate(island->springs[j],1.0/step,step);

	// elastic solver
	for (i=0; i<island->world_ref->elastic_iterations; i++)
	{
		// solve arbiters
		for (j=0; j<island->numarbs; j++)
			rat_arbiter_step_solver(island->arbiters[j],1.0);

		// solve constraints
		for (j=0; j<island->numconsts; j++)
			rat_constraint_step(island->constraints[j]);
	}

	// integrate velocity
	for (i=0; i<island->numbods; i++)
		rat_body_integrate_velocity(island->bodies[i],island->world_ref->gravity,step,island->world_ref->damping);

	// apply cached step
	for (i=0; i<island->numarbs; i++)
		rat_arbiter_step_cached(island->arbiters[i]);

	// inelastic solver
	for (i=0; i<island->world_ref->inelastic_iterations; i++)
	{
		// solve arbiters
		for (j=0; j<island->numarbs; j++)
			rat_arbiter_step_solver(island->arbiters[j],0.0);

		// solve constraints
		for (j=0; j<island->numconsts; j++)
			rat_constraint_step(island->constraints[j]);
	}

	// now we do island wide sleep checks
	for (i=0; i<island->numbods; i++)
	{
		rat_body *thisbody=island->bodies[i];
		if (thisbody->mass==0.0) continue;

		if (thisbody->omega*thisbody->omega>angular_tolerance_sq||
			vector2_dot(thisbody->velocity,thisbody->velocity)>linear_tolerance_sq)
		{
			thisbody->sleep_timer=0.0;
			min_sleep_time=0.0;
		}
		else
		{
			thisbody->sleep_timer+=step;
			min_sleep_time=min_sleep_time>thisbody->sleep_timer?thisbody->sleep_timer:min_sleep_time;
		}
	}

	// if everyone in the island is ready to sleep then put 'em to sleep
	if (min_sleep_time>=rat_time_to_sleep)
	{
		for (i=0; i<island->numbods; i++)
		{
			rat_body_set_is_asleep(island->bodies[i],1);
			vector2_init(&(island->bodies[i]->velocity));
			island->bodies[i]->omega=0.0;
		}
	}
}