// written in the D programming language

module chipmunkd.constraints.cpPivotJoint;

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

//const cpConstraintClass *cpPivotJointGetClass();

struct cpPivotJoint {
	cpConstraint constraint;
	cpVect anchr1, anchr2;
	
	cpVect r1, r2;
	cpVect k1, k2;
	
	cpVect jAcc;
	cpFloat jMaxLen;
	cpVect bias;
}

//cpPivotJoint *cpPivotJointAlloc(void);
//cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
//cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot);
//cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
//
//CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1);
//CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2);

static void
preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	joint.r1 = cpvrotate(joint.anchr1, a.rot);
	joint.r2 = cpvrotate(joint.anchr2, b.rot);
	
	// Calculate mass tensor
	k_tensor(a, b, joint.r1, joint.r2, &joint.k1, &joint.k2);
	
	// compute max impulse
	joint.jMaxLen = mixin(J_MAX!("joint", "dt"));
	
	// calculate bias velocity
	cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
	joint.bias = cpvclamp(cpvmult(delta, -joint.constraint.biasCoef*dt_inv), joint.constraint.maxBias);
	
	// apply accumulated impulse
	apply_impulses(a, b, joint.r1, joint.r2, joint.jAcc);
}

static void
applyImpulse(cpPivotJoint *joint)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	cpVect r1 = joint.r1;
	cpVect r2 = joint.r2;
		
	// compute relative velocity
	cpVect vr = relative_velocity(a, b, r1, r2);
	
	// compute normal impulse
	cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2);
	cpVect jOld = joint.jAcc;
	joint.jAcc = cpvclamp(cpvadd(joint.jAcc, j), joint.jMaxLen);
	j = cpvsub(joint.jAcc, jOld);
	
	// apply impulse
	apply_impulses(a, b, joint.r1, joint.r2, j);
}

static cpFloat
getImpulse(cpConstraint *joint)
{
	return cpvlength((cast(cpPivotJoint *)joint).jAcc);
}

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

cpPivotJoint *
cpPivotJointAlloc()
{
	return cast(cpPivotJoint *)cpmalloc(cpPivotJoint.sizeof);
}

cpPivotJoint *
cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.anchr1 = anchr1;
	joint.anchr2 = anchr2;
	
	joint.jAcc = cpvzero;
	
	return joint;
}

cpConstraint *
cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
{
	return cast(cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2);
}

cpConstraint *
cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
{
	cpVect anchr1 = (a ? cpBodyWorld2Local(a, pivot) : pivot);
	cpVect anchr2 = (b ? cpBodyWorld2Local(b, pivot) : pivot);
	return cpPivotJointNew2(a, b, anchr1, anchr2);
}