// written in the D programming language

module chipmunkd.constraints.cpDampedRotarySpring;

import chipmunkd.chipmunk;
import chipmunkd.constraints.util;

alias cpFloat function(cpConstraint *spring, cpFloat relativeAngle) cpDampedRotarySpringTorqueFunc;

//const cpConstraintClass *cpDampedRotarySpringGetClass();

struct cpDampedRotarySpring {
	cpConstraint constraint;
	cpFloat restAngle;
	cpFloat stiffness;
	cpFloat damping;
	cpDampedRotarySpringTorqueFunc springTorqueFunc;
	
	cpFloat target_wrn;
	cpFloat w_coef;
	
	cpFloat iSum;
}

//cpDampedRotarySpring *cpDampedRotarySpringAlloc(void);
//cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);
//cpConstraint *cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping);

mixin(CP_DefineConstraintProperty!("cpDampedRotarySpring", "cpFloat", "restAngle", "RestAngle"));
mixin(CP_DefineConstraintProperty!("cpDampedRotarySpring", "cpFloat", "stiffness", "Stiffness"));
mixin(CP_DefineConstraintProperty!("cpDampedRotarySpring", "cpFloat", "damping", "Damping"));
mixin(CP_DefineConstraintProperty!("cpDampedRotarySpring", "cpDampedRotarySpringTorqueFunc", "springTorqueFunc", "SpringTorqueFunc"));

// cpDampedRotarySpring.c ---------------------------------

static cpFloat
defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){
	return (relativeAngle - spring.restAngle)*spring.stiffness;
}

static void
preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
	
	cpFloat moment = a.i_inv + b.i_inv;
	spring.iSum = 1.0f/moment;

	spring.w_coef = 1.0f - cpfexp(-spring.damping*dt*moment);
	spring.target_wrn = 0.0f;

	// apply spring torque
	cpFloat j_spring = spring.springTorqueFunc(cast(cpConstraint *)spring, a.a - b.a)*dt;
	a.w -= j_spring*a.i_inv;
	b.w += j_spring*b.i_inv;
}

static void
applyImpulse(cpDampedRotarySpring *spring)
{
	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
	
	// compute relative velocity
	cpFloat wrn = a.w - b.w;//normal_relative_velocity(a, b, r1, r2, n) - spring.target_vrn;
	
	// compute velocity loss from drag
	// not 100% certain this is derived correctly, though it makes sense
	cpFloat w_damp = wrn*spring.w_coef;
	spring.target_wrn = wrn - w_damp;
	
	//apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, v_damp*spring.nMass));
	cpFloat j_damp = w_damp*spring.iSum;
	a.w -= j_damp*a.i_inv;
	b.w += j_damp*b.i_inv;
}

static cpFloat
getImpulse(cpConstraint *constraint)
{
	return 0.0f;
}

static /+const+/ cpConstraintClass klass = {
	cast(cpConstraintPreStepFunction)&preStep,
	cast(cpConstraintApplyImpulseFunction)&applyImpulse,
	cast(cpConstraintGetImpulseFunction)&getImpulse,
};
mixin(CP_DefineClassGetter!("cpDampedRotarySpring"));

cpDampedRotarySpring *
cpDampedRotarySpringAlloc()
{
	return cast(cpDampedRotarySpring *)cpmalloc(cpDampedRotarySpring.sizeof);
}

cpDampedRotarySpring *
cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
{
	cpConstraintInit(cast(cpConstraint *)spring, &klass, a, b);
	
	spring.restAngle = restAngle;
	spring.stiffness = stiffness;
	spring.damping = damping;
	spring.springTorqueFunc = cast(cpDampedRotarySpringTorqueFunc)&defaultSpringTorque;
	
	return spring;
}

cpConstraint *
cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping)
{
	return cast(cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping);
}