// written in the D programming language

module chipmunkd.constraints.cpSlideJoint;

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

//const cpConstraintClass *cpSlideJointGetClass();

struct cpSlideJoint {
	cpConstraint constraint;
	cpVect anchr1, anchr2;
	cpFloat min, max;
	
	cpVect r1, r2;
	cpVect n;
	cpFloat nMass;
	
	cpFloat jnAcc, jnMax;
	cpFloat bias;
}

//cpSlideJoint *cpSlideJointAlloc(void);
//cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
//cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max);
//
//CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1);
//CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2);
//CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min);
//CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max);

// cpSlideJoint.c ---------------------------------

static void
preStep(cpSlideJoint *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);
	cpFloat pdist = 0.0f;
	if(dist > joint.max) {
		pdist = dist - joint.max;
	} else if(dist < joint.min) {
		pdist = joint.min - dist;
		dist = -dist;
	}
	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*(pdist), -maxBias, maxBias);
	
	// compute max impulse
	joint.jnMax = mixin(J_MAX!("joint", "dt"));

	// apply accumulated impulse
	if(!joint.bias) //{
		// if bias is 0, then the joint is not at a limit.
		joint.jnAcc = 0.0f;
//	} else {
		cpVect j = cpvmult(joint.n, joint.jnAcc);
		apply_impulses(a, b, joint.r1, joint.r2, j);
//	}
}

static void
applyImpulse(cpSlideJoint *joint)
{
	if(!joint.bias) return;  // early exit

	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	cpVect n = joint.n;
	cpVect r1 = joint.r1;
	cpVect r2 = joint.r2;
		
	// compute relative velocity
	cpVect vr = relative_velocity(a, b, r1, r2);
	cpFloat vrn = cpvdot(vr, n);
	
	// compute normal impulse
	cpFloat jn = (joint.bias - vrn)*joint.nMass;
	cpFloat jnOld = joint.jnAcc;
	joint.jnAcc = cpfclamp(jnOld + jn, -joint.jnMax, 0.0f);
	jn = joint.jnAcc - jnOld;
	
	// apply impulse
	apply_impulses(a, b, joint.r1, joint.r2, cpvmult(n, jn));
}

static cpFloat
getImpulse(cpConstraint *joint)
{
	return cpfabs((cast(cpSlideJoint *)joint).jnAcc);
}

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

cpSlideJoint *
cpSlideJointAlloc()
{
	return cast(cpSlideJoint *)cpmalloc(cpSlideJoint.sizeof);
}

cpSlideJoint *
cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.anchr1 = anchr1;
	joint.anchr2 = anchr2;
	joint.min = min;
	joint.max = max;
	
	joint.jnAcc = 0.0f;
	
	return joint;
}

cpConstraint *
cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max)
{
	return cast(cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max);
}