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

static rat_real collision_slop=0.1;
static int split_impulse=1,warm_starting=1;

void rat_arbiter_global_set_slop(rat_real slop)			{collision_slop=fabs(slop);}
rat_real rat_arbiter_global_get_slop()					{return collision_slop;}

void rat_arbiter_global_split_impulse(int enable)		{split_impulse=enable?1:0;}
int rat_arbiter_global_split_impulse_enabled()			{return split_impulse;}

void rat_arbiter_global_warm_starting(int enable)		{warm_starting=enable?1:0;}
int rat_arbiter_global_warm_starting_enabled()			{return warm_starting;}

rat_arbiter *rat_arbiter_create(rat_block_allocator *bal,rat_body *body_a,rat_body *body_b,rat_contact_array *contacts)
{
	register unsigned int i;
	rat_arbiter *arbiter=(rat_arbiter *)rat_block_alloc(rat_global_block_allocator,sizeof(rat_arbiter));
	memset(arbiter,0,sizeof(rat_arbiter));

	arbiter->bal=bal;
	arbiter->stamped=1;

	arbiter->body_a=body_a;
	arbiter->body_b=body_b;

	rat_body_add_arbiter(body_a,arbiter);
	rat_body_add_arbiter(body_b,arbiter);

	arbiter->contacts=rat_alloc_contact_array(rat_global_block_allocator,rat_count_array_contacts(contacts));
	for (i=0; i<rat_count_array_contacts(contacts); i++)
		memcpy(rat_indexed_contact(arbiter->contacts,i),rat_indexed_contact(contacts,i),sizeof(rat_contact)-sizeof(rat_block_allocator *));

	return arbiter;
}

void rat_arbiter_destroy(rat_arbiter *arbiter)
{
	rat_body_remove_arbiter(arbiter->body_a,arbiter);
	rat_body_remove_arbiter(arbiter->body_b,arbiter);

	rat_free_contact_array(arbiter->contacts);
	rat_block_dealloc(rat_global_block_allocator,(void *)arbiter,sizeof(rat_arbiter));
}

void rat_arbiter_update_contacts(rat_arbiter *arbiter,rat_contact_array *new_contacts)
{
	rat_merge_contacts(&(arbiter->contacts),new_contacts);
}

void rat_arbiter_prime(rat_arbiter *arbiter,rat_real bias_coef)
{
	register unsigned int i,numcons;

	rat_body *body_a=arbiter->body_a;
	rat_body *body_b=arbiter->body_b;

	rat_real restitution=arbiter->body_a->elasticity*arbiter->body_b->elasticity;
	arbiter->netfriction=arbiter->body_a->friction*arbiter->body_b->friction;

	arbiter->stamped=0;

	rat_body_set_is_asleep(body_a,0);
	rat_body_set_is_asleep(body_b,0);

	numcons=rat_count_array_contacts(arbiter->contacts);
	for (i=0; i<numcons; i++)
	{
		rat_real mass_sum;
		rat_real off_a_cross_normal,off_b_cross_normal,normal_k;
		rat_real off_a_cross_tangent,off_b_cross_tangent,tangent_k;
		vector2 bounce_vel_a,bounce_vel_b,tangent;

		rat_contact *con=rat_indexed_contact(arbiter->contacts,i);

		con->off_a=vector2_sub(con->point,rat_hull_get_pos(body_a->hull));
		con->off_b=vector2_sub(con->point,rat_hull_get_pos(body_b->hull));

		con->bias_j=0.0;
		con->bias=bias_coef*(0.0<con->penetration+collision_slop?0.0:con->penetration+collision_slop);

		mass_sum=body_a->inv_mass+body_b->inv_mass;

		off_a_cross_normal=vector2_cross(con->off_a,con->normal);
		off_b_cross_normal=vector2_cross(con->off_b,con->normal);
		normal_k=mass_sum+
			body_a->inv_moi*off_a_cross_normal*off_a_cross_normal+
			body_b->inv_moi*off_b_cross_normal*off_b_cross_normal;

		tangent=vector2_perp_left(con->normal);
		off_a_cross_tangent=vector2_cross(con->off_a,tangent);
		off_b_cross_tangent=vector2_cross(con->off_b,tangent);
		tangent_k=mass_sum+
			body_a->inv_moi*off_a_cross_tangent*off_a_cross_tangent+
			body_b->inv_moi*off_b_cross_tangent*off_b_cross_tangent;

		con->mass_norm=normal_k==0.0?0.0:1.0/normal_k;
		con->mass_tan=tangent_k==0.0?0.0:1.0/tangent_k;

		bounce_vel_a=vector2_add(body_a->velocity,vector2_multf(vector2_perp_left(con->off_a),body_a->omega));
		bounce_vel_b=vector2_add(body_b->velocity,vector2_multf(vector2_perp_left(con->off_b),body_b->omega));
		con->bounce=vector2_dot(con->normal,vector2_sub(bounce_vel_b,bounce_vel_a))*restitution;
	}
}

void rat_arbiter_step_cached(rat_arbiter *arbiter)
{
	register unsigned int i,numcons;

	rat_body *body_a=arbiter->body_a;
	rat_body *body_b=arbiter->body_b;

	arbiter->netfriction=arbiter->body_a->friction*arbiter->body_b->friction;

	numcons=rat_count_array_contacts(arbiter->contacts);
	for	(i=0; i<numcons; i++)
	{
		rat_contact *con=rat_indexed_contact(arbiter->contacts,i);

		vector2 impulse=
			vector2_add(
				vector2_multf(
					con->normal,
					con->norm_acc_j),
				vector2_multf(
					vector2_perp_left(con->normal),
					con->tan_acc_j));

		rat_body_apply_offset_impulse(body_a,vector2_negative(impulse),con->off_a);
		rat_body_apply_offset_impulse(body_b,impulse,con->off_b);
	}
}

void rat_arbiter_step_solver(rat_arbiter *arbiter,rat_real restitution_coef)
{
	register unsigned int i,numcons;

	rat_body *body_a=arbiter->body_a;
	rat_body *body_b=arbiter->body_b;

	arbiter->netfriction=arbiter->body_a->friction*arbiter->body_b->friction;

	numcons=rat_count_array_contacts(arbiter->contacts);
	for	(i=0; i<numcons; i++)
	{
		vector2 vel_bias_a,vel_bias_b,bias_j;
		vector2 con_vel_a,con_vel_b,con_vel_ab;
		vector2 tangent,impulse;

		rat_real vel_bias_ab,norm_bias_j,old_norm_bias_j;
		rat_real con_vel_ab_norm,con_vel_ab_tan,norm_j;
		rat_real old_norm_j,tan_j_max,tan_j,old_tan_j;

		rat_contact *con=rat_indexed_contact(arbiter->contacts,i);

		if (split_impulse)
		{
			vel_bias_a=vector2_add(body_a->velocity_bias,vector2_multf(vector2_perp_left(con->off_a),body_a->omega_bias));
			vel_bias_b=vector2_add(body_b->velocity_bias,vector2_multf(vector2_perp_left(con->off_b),body_b->omega_bias));
			vel_bias_ab=vector2_dot(vector2_sub(vel_bias_b,vel_bias_a),con->normal);

			old_norm_bias_j=con->bias_j;
			norm_bias_j=(con->bias-vel_bias_ab)*con->mass_norm;
			con->bias_j=rat_maxf(old_norm_bias_j+norm_bias_j,0);
			norm_bias_j=con->bias_j-old_norm_bias_j;

			bias_j=vector2_multf(con->normal,norm_bias_j);

			rat_body_apply_offset_bias_impulse(body_a,vector2_negative(bias_j),con->off_a);
			rat_body_apply_offset_bias_impulse(body_b,bias_j,con->off_b);
		}

		con_vel_a=vector2_add(body_a->velocity,vector2_multf(vector2_perp_left(con->off_a),body_a->omega));
		con_vel_b=vector2_add(body_b->velocity,vector2_multf(vector2_perp_left(con->off_b),body_b->omega));
		con_vel_ab=vector2_sub(con_vel_b,con_vel_a);
		con_vel_ab_norm=vector2_dot(con_vel_ab,con->normal);

		norm_j=-(con->bounce*restitution_coef+con_vel_ab_norm)*con->mass_norm;
		old_norm_j=con->norm_acc_j;
		con->norm_acc_j=rat_maxf(old_norm_j+norm_j,0);
		norm_j=con->norm_acc_j-old_norm_j;

		tangent=vector2_perp_left(con->normal);
		con_vel_ab_tan=vector2_dot(con_vel_ab,tangent);

		tan_j_max=arbiter->netfriction*con->norm_acc_j;
		tan_j=-con_vel_ab_tan*con->mass_tan;
		old_tan_j=con->tan_acc_j;
		con->tan_acc_j=rat_clampf(old_tan_j+tan_j,-tan_j_max,tan_j_max);
		tan_j=con->tan_acc_j-old_tan_j;

		impulse=vector2_add(vector2_multf(con->normal,norm_j),vector2_multf(tangent,tan_j));
		rat_body_apply_offset_impulse(body_a,vector2_negative(impulse),con->off_a);
		rat_body_apply_offset_impulse(body_b,impulse,con->off_b);
	}
}