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

rat_real rat_linear_sleep_tolerance=0.5;
rat_real rat_angular_sleep_tolerance=0.06; // about 2 degrees
rat_real rat_time_to_sleep=2.5;

rat_body *rat_body_create(rat_hull *hull,rat_real elasticity,rat_real friction,rat_real mass,rat_real moi)
{
	rat_body *body=(rat_body *)rat_alloc(sizeof(rat_body));
	memset(body,0,sizeof(rat_body));

	body->userdata=NULL;
	body->filtertag=0;
	body->hull=hull;

	rat_body_set_mass(body,mass);
	rat_body_set_moi(body,moi);

	body->elasticity=elasticity;
	body->friction=friction;

	return body;
}

void rat_body_destroy(rat_body *body)
{
	rat_hull_destroy(body->hull);
	if (body->numsprings) rat_dealloc((void *)body->springs);
	if (body->numarbiters) rat_dealloc((void *)body->arbiters);
	if (body->numconsts) rat_dealloc((void *)body->constraints);
	rat_dealloc((void *)body);
}

void rat_body_set_is_bullet(rat_body *body,int is_bullet)
{
	if (is_bullet)
		body->body_flags|=rat_is_bullet_flag;
	else
		body->body_flags&=~rat_is_bullet_flag;
}

int rat_body_get_is_bullet(rat_body *body)
{
	return body->body_flags&rat_is_bullet_flag;
}

void rat_body_set_is_asleep(rat_body *body,int is_asleep)
{
	if (is_asleep)
	{
		register unsigned int i;
		body->body_flags|=rat_is_asleep_flag;
	}
	else
	{
		if (body->body_flags&rat_is_asleep_flag) body->sleep_timer=0.0;
		body->body_flags&=~rat_is_asleep_flag;
	}
}

int rat_body_get_is_asleep(rat_body *body)
{
	return body->body_flags&rat_is_asleep_flag;
}

void rat_body_set_island_marker(rat_body *body)
{
	body->body_flags|=rat_island_flag;
}

void rat_body_kill_island_marker(rat_body *body)
{
	body->body_flags&=~rat_island_flag;
}

int rat_body_get_island_marker(rat_body *body)
{
	return body->body_flags&rat_island_flag;
}

void rat_body_enable_adjacency_filter(rat_body *body)
{
	body->body_flags|=rat_adjacency_filter_flag;
}

void rat_body_disable_adjacency_filter(rat_body *body)
{
	body->body_flags&=~rat_adjacency_filter_flag;
}

int rat_body_is_adjacency_filter_enabled(rat_body *body)
{
	return body->body_flags&rat_adjacency_filter_flag;
}

void rat_body_set_filter_tag(rat_body *body,int tag)
{
	body->filtertag=tag;
}

int rat_body_get_filter_tag(rat_body *body)
{
	return body->filtertag;
}

void rat_body_set_mass(rat_body *body,rat_real mass)
{
	body->mass=mass;
	body->inv_mass=mass==0.0?0.0:1.0/mass;
}

void rat_body_set_moi(rat_body *body,rat_real moi)
{
	body->moi=moi;
	body->inv_moi=moi==0.0?0.0:1.0/moi;
}

rat_real rat_body_get_mass(rat_body *body)		{return body->mass;}
rat_real rat_body_get_moi(rat_body *body)		{return body->moi;}

vector2 rat_body_get_velocity(rat_body *body)	{return body->velocity;}
rat_real rat_body_get_omega(rat_body *body)		{return body->omega;}

void rat_body_apply_offset_impulse(rat_body *body,vector2 impulse,vector2 offset)
{
	vector2 dvel=vector2_multf(impulse,body->inv_mass);
	rat_real dvsq=vector2_dot(dvel,dvel);
	rat_real dw=body->inv_moi*vector2_cross(offset,impulse);

	if (dvsq>rat_linear_sleep_tolerance*rat_linear_sleep_tolerance||
		dw*dw>rat_angular_sleep_tolerance*rat_angular_sleep_tolerance)
		rat_body_set_is_asleep(body,0);

	vector2_plus(&(body->velocity),dvel);
	body->omega+=dw;
}

void rat_body_apply_impulse(rat_body *body,vector2 impulse)
{
	vector2 dvel=vector2_multf(impulse,body->inv_mass);
	rat_real dvsq=vector2_dot(dvel,dvel);

	if (dvsq>rat_linear_sleep_tolerance*rat_linear_sleep_tolerance)
		rat_body_set_is_asleep(body,0);

	vector2_plus(&(body->velocity),dvel);
}

void rat_body_apply_torque(rat_body *body,rat_real torque)
{
	rat_real dw=body->inv_moi*torque;

	if (dw*dw>rat_angular_sleep_tolerance*rat_angular_sleep_tolerance)
		rat_body_set_is_asleep(body,0);

	body->omega+=dw;
}

void rat_body_apply_offset_bias_impulse(rat_body *body,vector2 impulse,vector2 offset)
{
	vector2 dvel=vector2_multf(impulse,body->inv_mass);
	rat_real dvsq=vector2_dot(dvel,dvel);
	rat_real dw=body->inv_moi*vector2_cross(offset,impulse);

	vector2_plus(&(body->velocity_bias),dvel);
	body->omega_bias+=dw;
}

void rat_body_apply_bias_impulse(rat_body *body,vector2 impulse)
{
	vector2 dvel=vector2_multf(impulse,body->inv_mass);
	rat_real dvsq=vector2_dot(dvel,dvel);

	vector2_plus(&(body->velocity_bias),dvel);
}

void rat_body_apply_bias_torque(rat_body *body,rat_real torque)
{
	rat_real dw=body->inv_moi*torque;

	body->omega_bias+=dw;
}

void rat_body_apply_force(rat_body *body,vector2 force,vector2 offset)
{
	vector2 dvel=vector2_multf(force,body->inv_mass);
	rat_real dvsq=vector2_dot(dvel,dvel);
	rat_real dw=body->inv_moi*vector2_cross(offset,force);

	if (dvsq>rat_linear_sleep_tolerance*rat_linear_sleep_tolerance||
		dw*dw>rat_angular_sleep_tolerance*rat_angular_sleep_tolerance)
		rat_body_set_is_asleep(body,0);

	vector2_plus(&(body->force),force);
	body->torque+=vector2_cross(offset,force);
}

void rat_body_apply_gravity(rat_body *body,vector2 gravity)
{
	vector2 zerovec;

	vector2_init(&zerovec);
	rat_body_apply_force(body,vector2_multf(gravity,body->mass),zerovec);
}

void rat_body_clear_force(rat_body *body)
{
	body->force.x=0;
	body->force.y=0;
	body->torque=0;
}

void rat_body_integrate_velocity(rat_body *body,vector2 gravity,rat_real timestep,rat_real damping)
{
	register rat_real proportional_damping=powf(damping==0.0?0.0:1.0/damping,-timestep);

	// apply damping and impulse from net force
	vector2_plus(&(body->velocity),vector2_add(vector2_multf(vector2_multf(body->force,body->inv_mass),timestep),vector2_multf(gravity,timestep)));
	vector2_timesf(&(body->velocity),proportional_damping);

	// apply damping and torque from net force
	body->omega+=body->torque*body->inv_moi*timestep;
	body->omega*=proportional_damping;

	rat_body_clear_force(body);
}

void rat_body_integrate_position_euler(rat_body *body,rat_real timestep)
{
#ifdef RAT_PHYSICS_MSVC
	rat_real vars[9];
#else
	rat_real *vars=(rat_real *)rat_alloc(9*sizeof(rat_real));
#endif
	rat_real *in=vars,*out=vars+3,*chg=vars+6;

	vector2 *pos=(vector2 *)in;
	vector2 *vel=(vector2 *)chg;

	rat_real *angle=in+2;
	rat_real *omega=chg+2;

	*pos=rat_hull_get_pos(body->hull);
	*angle=rat_hull_get_angle(body->hull);
	*vel=vector2_add(body->velocity,body->velocity_bias);
	*omega=body->omega+body->omega_bias;

	// i can has a cheezy euler integrator!1!11!!
	rat_math_util_euler_integrate_real(3,in,out,chg,timestep);

	pos=(vector2 *)out;
	angle=out+2;

	rat_hull_set_pos(body->hull,*pos);
	rat_hull_set_angle(body->hull,*angle);

	vector2_set(&(body->velocity_bias),0.0,0.0);
	body->omega_bias=0.0;

#ifndef RAT_PHYSICS_MSVC
	rat_dealloc((void *)vars);
#endif
}

void rat_body_integrate_position_rk4(rat_body *body,rat_real timestep)
{
#ifdef RAT_PHYSICS_MSVC
	rat_real vars[9];
#else
	rat_real *vars=(rat_real *)rat_alloc(9*sizeof(rat_real));
#endif
	rat_real *in=vars,*out=vars+3,*chg=vars+6;

	vector2 *pos=(vector2 *)in;
	vector2 *vel=(vector2 *)chg;

	rat_real *angle=in+2;
	rat_real *omega=chg+2;

	*pos=rat_hull_get_pos(body->hull);
	*angle=rat_hull_get_angle(body->hull);
	*vel=vector2_add(body->velocity,body->velocity_bias);
	*omega=body->omega+body->omega_bias;

	rat_math_util_rk4_integrate_real(3,in,out,chg,timestep);

	pos=(vector2 *)out;
	angle=out+2;

	rat_hull_set_pos(body->hull,*pos);
	rat_hull_set_angle(body->hull,*angle);

	vector2_set(&(body->velocity_bias),0.0,0.0);
	body->omega_bias=0.0;

#ifndef RAT_PHYSICS_MSVC
	rat_dealloc((void *)vars);
#endif
}

void rat_body_add_arbiter(rat_body *body,rat_arbiter *arbiter)
{
	if (body->numarbiters)
	{
		body->arbiters=(rat_arbiter **)rat_realloc(body->arbiters,(body->numarbiters+1)*sizeof(rat_arbiter *));
		body->numarbiters+=1;
	}
	else
	{
		body->arbiters=(rat_arbiter **)rat_alloc(sizeof(rat_arbiter *));
		body->numarbiters=1;
	}
	body->arbiters[body->numarbiters-1]=arbiter;

	rat_body_set_is_asleep(body,0);
}

void rat_body_remove_arbiter(rat_body *body,rat_arbiter *arbiter)
{
	register unsigned int i,j;
	for (i=0; i<body->numarbiters; i++)
	{
		if (body->arbiters[i]==arbiter)
		{
			for (j=i+1; j<body->numarbiters; j++)
				body->arbiters[j-1]=body->arbiters[j];

			body->numarbiters--;
			if (body->numarbiters)
				body->arbiters=(rat_arbiter **)rat_realloc(body->arbiters,body->numarbiters*sizeof(rat_arbiter *));
			else
				rat_dealloc((void *)body->arbiters);
		}
	}

	rat_body_set_is_asleep(body,0);
}

void rat_body_add_spring(rat_body *body,rat_spring *spring)
{
	if (body->numsprings)
	{
		body->springs=(rat_spring **)rat_realloc(body->springs,(body->numsprings+1)*sizeof(rat_spring *));
		body->numsprings+=1;
	}
	else
	{
		body->springs=(rat_spring **)rat_alloc(sizeof(rat_spring *));
		body->numsprings=1;
	}
	body->springs[body->numsprings-1]=spring;

	rat_body_set_is_asleep(body,0);
}

void rat_body_add_constraint(rat_body *body,rat_constraint *constraint)
{
	if (body->numconsts)
	{
		body->constraints=(rat_constraint **)rat_realloc(body->constraints,(body->numconsts+1)*sizeof(rat_constraint *));
		body->numconsts+=1;
	}
	else
	{
		body->constraints=(rat_constraint **)rat_alloc(sizeof(rat_constraint *));
		body->numconsts=1;
	}
	body->constraints[body->numconsts-1]=constraint;

	rat_body_set_is_asleep(body,0);
}

void rat_body_remove_spring(rat_body *body,rat_spring *spring)
{
	register unsigned int i,j;
	for (i=0; i<body->numsprings; i++)
	{
		if (body->springs[i]==spring)
		{
			for (j=i+1; j<body->numsprings; j++)
				body->springs[j-1]=body->springs[j];

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

	rat_body_set_is_asleep(body,0);
}

void rat_body_remove_constraint(rat_body *body,rat_constraint *constraint)
{
	register unsigned int i,j;
	for (i=0; i<body->numconsts; i++)
	{
		if (body->constraints[i]==constraint)
		{
			for (j=i+1; j<body->numconsts; j++)
				body->constraints[j-1]=body->constraints[j];

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

	rat_body_set_is_asleep(body,0);
}

void rat_body_destroy_springs(rat_body *body,rat_world *world)
{
	register unsigned int i;
	for (i=0; i<body->numsprings; i++)
		rat_world_remove_and_destroy_spring(world,body->springs[i]);
}

void rat_body_destroy_constraints(rat_body *body,rat_world *world)
{
	register unsigned int i;
	for (i=0; i<body->numconsts; i++)
		rat_world_remove_and_destroy_constraint(world,body->constraints[i]);
}