// written in the D programming language

module chipmunkd.constraints.cpRatchetJoint;

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

//const cpConstraintClass *cpRatchetJointGetClass();

struct cpRatchetJoint {
	cpConstraint constraint;
	cpFloat angle, phase, ratchet;
	
	cpFloat iSum;
		
	cpFloat bias;
	cpFloat jAcc, jMax;
}

//cpRatchetJoint *cpRatchetJointAlloc(void);
//cpRatchetJoint *cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
//cpConstraint *cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet);
//
//CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, angle, Angle);
//CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, phase, Phase);
//CP_DefineConstraintProperty(cpRatchetJoint, cpFloat, ratchet, Ratchet);

// cpRatchetJoint.c ---------------------------------

static void
preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	cpFloat angle = joint.angle;
	cpFloat phase = joint.phase;
	cpFloat ratchet = joint.ratchet;
	
	cpFloat delta = b.a - a.a;
	cpFloat diff = angle - delta;
	cpFloat pdist = 0.0f;
	
	if(diff*ratchet > 0.0f){
		pdist = diff;
	} else {
		joint.angle = cpffloor((delta - phase)/ratchet)*ratchet + phase;
	}
	
	// calculate moment of inertia coefficient.
	joint.iSum = 1.0f/(a.i_inv + b.i_inv);
	
	// calculate bias velocity
	cpFloat maxBias = joint.constraint.maxBias;
	joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias);
	
	// compute max impulse
	joint.jMax = mixin(J_MAX!("joint", "dt"));

	// If the bias is 0, the joint is not at a limit. Reset the impulse.
	if(!joint.bias)
		joint.jAcc = 0.0f;

	// apply joint torque
	a.w -= joint.jAcc*a.i_inv;
	b.w += joint.jAcc*b.i_inv;
}

static void
applyImpulse(cpRatchetJoint *joint)
{
	if(!joint.bias) return; // early exit

	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	// compute relative rotational velocity
	cpFloat wr = b.w - a.w;
	cpFloat ratchet = joint.ratchet;
	
	// compute normal impulse	
	cpFloat j = -(joint.bias + wr)*joint.iSum;
	cpFloat jOld = joint.jAcc;
	joint.jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint.jMax*cpfabs(ratchet))/ratchet;
	j = joint.jAcc - jOld;
	
	// apply impulse
	a.w -= j*a.i_inv;
	b.w += j*b.i_inv;
}

static cpFloat
getImpulse(cpRatchetJoint *joint)
{
	return cpfabs(joint.jAcc);
}

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

cpRatchetJoint *
cpRatchetJointAlloc()
{
	return cast(cpRatchetJoint *)cpmalloc(cpRatchetJoint.sizeof);
}

cpRatchetJoint *
cpRatchetJointInit(cpRatchetJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.angle = 0.0f;
	joint.phase = phase;
	joint.ratchet = ratchet;
	
	// STATIC_BODY_CHECK
	joint.angle = (b ? b.a : 0.0f) - (a ? a.a : 0.0f);
	
	return joint;
}

cpConstraint *
cpRatchetJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratchet)
{
	return cast(cpConstraint *)cpRatchetJointInit(cpRatchetJointAlloc(), a, b, phase, ratchet);
}