// written in the D programming language

module chipmunkd.constraints.cpPinJoint;

import chipmunkd.chipmunk;
import chipmunkd.chipmunk_types_h;
import chipmunkd.cpVect,chipmunkd.cpVect_h;
import chipmunkd.cpBody;
import chipmunkd.constraints.util;

//const cpConstraintClass *cpPinJointGetClass();

struct cpPinJoint {
	cpConstraint constraint;
	cpVect anchr1, anchr2;
	cpFloat dist;
	
	cpVect r1, r2;
	cpVect n;
	cpFloat nMass;
	
	cpFloat jnAcc, jnMax;
	cpFloat bias;
}

//cpPinJoint *cpPinJointAlloc(void);
//cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
//cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2);
//
//CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1);
//CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2);
//CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist);

// cpPinJoint.c -------------------------

static void
preStep(cpPinJoint *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);
	
	cpVect delta = cpvsub(cpvadd(b.p, joint.r2), cpvadd(a.p, joint.r1));
	cpFloat dist = cpvlength(delta);
	joint.n = cpvmult(delta, 1.0f/(dist ? dist : cast(cpFloat)INFINITY));
	
	// calculate mass normal
	joint.nMass = 1.0f/k_scalar(a, b, joint.r1, joint.r2, joint.n);
	
	// calculate bias velocity
	cpFloat maxBias = joint.constraint.maxBias;
	joint.bias = cpfclamp(-joint.constraint.biasCoef*dt_inv*(dist - joint.dist), -maxBias, maxBias);
	
	// compute max impulse
	joint.jnMax = mixin(J_MAX!("joint", "dt"));
	
	// apply accumulated impulse
	cpVect j = cpvmult(joint.n, joint.jnAcc);
	apply_impulses(a, b, joint.r1, joint.r2, j);
}

static void
applyImpulse(cpPinJoint *joint)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	cpVect n = joint.n;

	// compute relative velocity
	cpFloat vrn = normal_relative_velocity(a, b, joint.r1, joint.r2, n);
	
	// compute normal impulse
	cpFloat jn = (joint.bias - vrn)*joint.nMass;
	cpFloat jnOld = joint.jnAcc;
	joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, joint.jnMax);
	jn = joint.jnAcc - jnOld;
	
	// apply impulse
	apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn));
}

static cpFloat
getImpulse(cpPinJoint *joint)
{
	return cpfabs(joint.jnAcc);
}

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

cpPinJoint *
cpPinJointAlloc()
{
	return cast(cpPinJoint *)cpmalloc(cpPinJoint.sizeof);
}

cpPinJoint *
cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.anchr1 = anchr1;
	joint.anchr2 = anchr2;
	
	// STATIC_BODY_CHECK
	cpVect p1 = (a ? cpvadd(a.p, cpvrotate(anchr1, a.rot)) : anchr1);
	cpVect p2 = (b ? cpvadd(b.p, cpvrotate(anchr2, b.rot)) : anchr2);
	joint.dist = cpvlength(cpvsub(p2, p1));

	joint.jnAcc = 0.0f;
	
	return joint;
}

cpConstraint *
cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
{
	return cast(cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2);
}