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

rat_spring *rat_spring_create(rat_body *body_a,rat_body *body_b,
	vector2 anch_a,vector2 anch_b,rat_real k,rat_real rest_length,
	rat_real damping)
{
	rat_spring *spring=(rat_spring *)rat_alloc(sizeof(rat_spring));
	if (!body_a)
	{
		rat_dealloc((void *)spring);
		return NULL;
	}

	spring->body_a=body_a;
	spring->anch_a=anch_a;

	spring->body_b=body_b;
	spring->anch_b=anch_b;

	spring->k=k;
	spring->rest_length=rest_length;
	spring->damping=damping;

	spring->activity=RAT_SPRING_ACTIVITY_NEUTRAL;

	if (body_a) rat_body_add_spring(body_a,spring);
	if (body_b) rat_body_add_spring(body_b,spring);

	return spring;
}

void rat_spring_destroy(rat_spring *spring)
{
	if (spring->body_a) rat_body_remove_spring(spring->body_a,spring);
	if (spring->body_b) rat_body_remove_spring(spring->body_b,spring);

	rat_dealloc((void *)spring);
}

void rat_spring_update_global_anchor(rat_spring *spring,vector2 new_anch_b)
{
	spring->anch_b=new_anch_b;
}

void rat_spring_integrate(rat_spring *spring,rat_real inv_timestep,rat_real solve_coef)
{
	register rat_real proportional_damping=powf(spring->damping==0.0?0.0:1.0/spring->damping,-solve_coef);
	register vector2 zero_vec={0,0};
	rat_real force,distance;
	vector2 impulse,delta,normal;

	if (!spring->body_a) return;

	if (spring->body_b)
	{
		vector2 world_anch_a=vector2_getrotated(spring->anch_a,rat_hull_get_angle(spring->body_a->hull));
		vector2 world_anch_b=vector2_getrotated(spring->anch_b,rat_hull_get_angle(spring->body_b->hull));
		
		vector2 delta=vector2_sub(vector2_add(rat_hull_get_pos(spring->body_b->hull),world_anch_b),vector2_add(rat_hull_get_pos(spring->body_a->hull),world_anch_a));
		rat_real distance=vector2_magnitude(delta);
		vector2 normal=distance!=0?vector2_multf(delta,1.0f/distance):zero_vec;
		
		force=-(distance-spring->rest_length)*spring->k*inv_timestep;
		if (force>0)
			force-=rat_maxf(fabs(force),proportional_damping);
		else
			force+=rat_maxf(fabs(force),proportional_damping);
		spring->activity=force>0?RAT_SPRING_ACTIVITY_PUSHING:RAT_SPRING_ACTIVITY_PULLING;
		impulse=vector2_multf(normal,force*solve_coef);

		rat_body_apply_offset_impulse(spring->body_a,vector2_negative(impulse),world_anch_a);
		rat_body_apply_offset_impulse(spring->body_b,impulse,world_anch_b);
	}
	else // second anchor is global
	{
		vector2 world_anch_a=vector2_getrotated(spring->anch_a,rat_hull_get_angle(spring->body_a->hull));
		
		vector2 delta=vector2_sub(spring->anch_b,vector2_add(rat_hull_get_pos(spring->body_a->hull),world_anch_a));
		rat_real distance=vector2_magnitude(delta);
		vector2 normal=distance?vector2_multf(delta,1.0f/distance):zero_vec;
		
		force=-(distance-spring->rest_length)*spring->k*inv_timestep;
		if (force>0)
			force-=rat_maxf(fabs(force),proportional_damping);
		else
			force+=rat_maxf(fabs(force),proportional_damping);
		spring->activity=force>0?RAT_SPRING_ACTIVITY_PUSHING:RAT_SPRING_ACTIVITY_PULLING;
		impulse=vector2_multf(normal,force*solve_coef);

		rat_body_apply_offset_impulse(spring->body_a,vector2_negative(impulse),world_anch_a);

	}
}