// written in the D programming language

module chipmunkd.constraints.cpGearJoint;

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

//const cpConstraintClass *cpGearJointGetClass();

struct cpGearJoint {
	cpConstraint constraint;
	cpFloat phase, ratio;
	cpFloat ratio_inv;
	
	cpFloat iSum;
		
	cpFloat bias;
	cpFloat jAcc, jMax;
}

//cpGearJoint *cpGearJointAlloc(void);
//cpGearJoint *cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio);
//cpConstraint *cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio);
//
//CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase);
//CP_DefineConstraintGetter(cpGearJoint, cpFloat, ratio, Ratio);
//void cpGearJointSetRatio(cpConstraint *constraint, cpFloat value);

// cpGearJoint.c ---------------------------------

static void
preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	// calculate moment of inertia coefficient.
	joint.iSum = 1.0f/(a.i_inv*joint.ratio_inv + joint.ratio*b.i_inv);
	
	// calculate bias velocity
	cpFloat maxBias = joint.constraint.maxBias;
	joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(b.a*joint.ratio - a.a - joint.phase), -maxBias, maxBias);
	
	// compute max impulse
	joint.jMax = mixin(J_MAX!("joint", "dt"));

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

static void
applyImpulse(cpGearJoint *joint)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	// compute relative rotational velocity
	cpFloat wr = b.w*joint.ratio - a.w;
	
	// compute normal impulse	
	cpFloat j = (joint.bias - wr)*joint.iSum;
	cpFloat jOld = joint.jAcc;
	joint.jAcc = cpfclamp(jOld + j, -joint.jMax, joint.jMax);
	j = joint.jAcc - jOld;
	
	// apply impulse
	a.w -= j*a.i_inv*joint.ratio_inv;
	b.w += j*b.i_inv;
}

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

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

cpGearJoint *
cpGearJointAlloc()
{
	return cast(cpGearJoint *)cpmalloc(cpGearJoint.sizeof);
}

cpGearJoint *
cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.phase = phase;
	joint.ratio = ratio;
	joint.ratio_inv = 1.0f/ratio;
	
	joint.jAcc = 0.0f;
	
	return joint;
}

cpConstraint *
cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio)
{
	return cast(cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio);
}

void
cpGearJointSetRatio(cpConstraint *constraint, cpFloat value)
{
	//TODO:
	//cpConstraintCheckCast(constraint, cpGearJoint);
	(cast(cpGearJoint *)constraint).ratio = value;
	(cast(cpGearJoint *)constraint).ratio_inv = 1.0f/value;
	cpConstraintActivateBodies(constraint);
}