// written in the D programming language

module chipmunkd.constraints.cpRotaryLimitJoint;

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

//const cpConstraintClass *cpRotaryLimitJointGetClass();

struct cpRotaryLimitJoint {
	cpConstraint constraint;
	cpFloat min, max;
	
	cpFloat iSum;
		
	cpFloat bias;
	cpFloat jAcc, jMax;
}

//cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void);
//cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max);
//cpConstraint *cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max);
//
//CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min);
//CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max);

// cpRotaryLimitJoint.c ---------------------------------

static void
preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	cpFloat dist = b.a - a.a;
	cpFloat pdist = 0.0f;
	if(dist > joint.max) {
		pdist = joint.max - dist;
	} else if(dist < joint.min) {
		pdist = joint.min - dist;
	}
	
	// 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(cpRotaryLimitJoint *joint)
{
	if(!joint.bias) return; // early exit

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

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

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

cpRotaryLimitJoint *
cpRotaryLimitJointAlloc()
{
	return cast(cpRotaryLimitJoint *)cpmalloc(cpRotaryLimitJoint.sizeof);
}

cpRotaryLimitJoint *
cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.min = min;
	joint.max  = max;
	
	joint.jAcc = 0.0f;
	
	return joint;
}

cpConstraint *
cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max)
{
	return cast(cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max);
}