// written in the D programming language

module chipmunkd.constraints.cpDampedSpring;

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

alias cpFloat function(cpConstraint *spring, cpFloat dist) cpDampedSpringForceFunc;

//const cpConstraintClass *cpDampedSpringGetClass();

struct cpDampedSpring {
	cpConstraint constraint;
	cpVect anchr1, anchr2;
	cpFloat restLength;
	cpFloat stiffness;
	cpFloat damping;
	cpDampedSpringForceFunc springForceFunc;
	
	cpFloat target_vrn;
	cpFloat v_coef;
	
	cpVect r1, r2;
	cpFloat nMass;
	cpVect n;
}

//cpDampedSpring *cpDampedSpringAlloc(void);
//cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);
//cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping);

mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr1", "Anchr1"));
mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpVect", "anchr2", "Anchr2"));
mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "restLength", "RestLength"));
mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "stiffness", "Stiffness"));
mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpFloat", "damping", "Damping"));
mixin(CP_DefineConstraintProperty!("cpDampedSpring", "cpDampedSpringForceFunc", "springForceFunc", "SpringForceFunc"));

static cpFloat
defaultSpringForce(cpDampedSpring *spring, cpFloat dist){
	return (spring.restLength - dist)*spring.stiffness;
}

static void
preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
	
	spring.r1 = cpvrotate(spring.anchr1, a.rot);
	spring.r2 = cpvrotate(spring.anchr2, b.rot);
	
	cpVect delta = cpvsub(cpvadd(b.p, spring.r2), cpvadd(a.p, spring.r1));
	cpFloat dist = cpvlength(delta);
	spring.n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
	
	cpFloat k = k_scalar(a, b, spring.r1, spring.r2, spring.n);
	spring.nMass = 1.0f/k;
	
	spring.target_vrn = 0.0f;
	spring.v_coef = 1.0f - cpfexp(-spring.damping*dt*k);

	// apply spring force
	cpFloat f_spring = spring.springForceFunc(cast(cpConstraint *)spring, dist);
	apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, f_spring*dt));
}

static void
applyImpulse(cpDampedSpring *spring)
{
	mixin(CONSTRAINT_BEGIN!("spring", "a", "b"));
	
	cpVect n = spring.n;
	cpVect r1 = spring.r1;
	cpVect r2 = spring.r2;

	// compute relative velocity
	cpFloat vrn = 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 v_damp = -vrn*spring.v_coef;
	spring.target_vrn = vrn + v_damp;
	
	apply_impulses(a, b, spring.r1, spring.r2, cpvmult(spring.n, v_damp*spring.nMass));
}

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

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

cpDampedSpring *
cpDampedSpringAlloc()
{
	return cast(cpDampedSpring *)cpmalloc(cpDampedSpring.sizeof);
}

cpDampedSpring *
cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
{
	cpConstraintInit(cast(cpConstraint *)spring, cpDampedSpringGetClass(), a, b);
	
	spring.anchr1 = anchr1;
	spring.anchr2 = anchr2;
	
	spring.restLength = restLength;
	spring.stiffness = stiffness;
	spring.damping = damping;
	spring.springForceFunc = cast(cpDampedSpringForceFunc)&defaultSpringForce;
	
	return spring;
}

cpConstraint *
cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping)
{
	return cast(cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping);
}