// written in the D programming language

module chipmunkd.constraints.cpSimpleMotor;

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

//const cpConstraintClass *cpSimpleMotorGetClass();

struct cpSimpleMotor {
	cpConstraint constraint;
	cpFloat rate;
	
	cpFloat iSum;
		
	cpFloat jAcc, jMax;
}

//cpSimpleMotor *cpSimpleMotorAlloc(void);
//cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate);
//cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate);
//
//CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate);

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

static void
preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	// calculate moment of inertia coefficient.
	joint.iSum = 1.0f/(a.i_inv + b.i_inv);
	
	// compute max impulse
	joint.jMax = mixin(J_MAX!("joint", "dt"));

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

static void
applyImpulse(cpSimpleMotor *joint)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	// compute relative rotational velocity
	cpFloat wr = b.w - a.w + joint.rate;
	
	// compute normal impulse	
	cpFloat j = -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;
	b.w += j*b.i_inv;
}

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

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

cpSimpleMotor *
cpSimpleMotorAlloc()
{
	return cast(cpSimpleMotor *)cpmalloc(cpSimpleMotor.sizeof);
}

cpSimpleMotor *
cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.rate = rate;
	
	joint.jAcc = 0.0f;
	
	return joint;
}

cpConstraint *
cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate)
{
	return cast(cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate);
}