// written in the D programming language

module chipmunkd.constraints.cpGrooveJoint;

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

//const cpConstraintClass *cpGrooveJointGetClass();

struct cpGrooveJoint {
	cpConstraint constraint;
	cpVect grv_n, grv_a, grv_b;
	cpVect  anchr2;
	
	cpVect grv_tn;
	cpFloat clamp;
	cpVect r1, r2;
	cpVect k1, k2;
	
	cpVect jAcc;
	cpFloat jMaxLen;
	cpVect bias;
}

//cpGrooveJoint *cpGrooveJointAlloc(void);
//cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
//cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2);
//
//
//CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_a, GrooveA);
//void cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value);
//CP_DefineConstraintGetter(cpGrooveJoint, cpVect, grv_b, GrooveB);
//void cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value);
//CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2);

// cpGrooveJoint.c ---------------------------------

static void
preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	// calculate endpoints in worldspace
	cpVect ta = cpBodyLocal2World(a, joint.grv_a);
	cpVect tb = cpBodyLocal2World(a, joint.grv_b);

	// calculate axis
	cpVect n = cpvrotate(joint.grv_n, a.rot);
	cpFloat d = cpvdot(ta, n);
	
	joint.grv_tn = n;
	joint.r2 = cpvrotate(joint.anchr2, b.rot);
	
	// calculate tangential distance along the axis of r2
	cpFloat td = cpvcross(cpvadd(b.p, joint.r2), n);
	// calculate clamping factor and r2
	if(td <= cpvcross(ta, n)){
		joint.clamp = 1.0f;
		joint.r1 = cpvsub(ta, a.p);
	} else if(td >= cpvcross(tb, n)){
		joint.clamp = -1.0f;
		joint.r1 = cpvsub(tb, a.p);
	} else {
		joint.clamp = 0.0f;
		joint.r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a.p);
	}
	
	// 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 cpVect
grooveConstrain(cpGrooveJoint *joint, cpVect j){
	cpVect n = joint.grv_tn;
	cpVect jClamp = (joint.clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
	return cpvclamp(jClamp, joint.jMaxLen);
}

static void
applyImpulse(cpGrooveJoint *joint)
{
	mixin(CONSTRAINT_BEGIN!("joint", "a", "b"));
	
	cpVect r1 = joint.r1;
	cpVect r2 = joint.r2;
	
	// compute impulse
	cpVect vr = relative_velocity(a, b, r1, r2);

	cpVect j = mult_k(cpvsub(joint.bias, vr), joint.k1, joint.k2);
	cpVect jOld = joint.jAcc;
	joint.jAcc = grooveConstrain(joint, cpvadd(jOld, j));
	j = cpvsub(joint.jAcc, jOld);
	
	// apply impulse
	apply_impulses(a, b, joint.r1, joint.r2, j);
}

static cpFloat
getImpulse(cpGrooveJoint *joint)
{
	return cpvlength(joint.jAcc);
}

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

cpGrooveJoint *
cpGrooveJointAlloc()
{
	return cast(cpGrooveJoint *)cpmalloc(cpGrooveJoint.sizeof);
}

cpGrooveJoint *
cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
{
	cpConstraintInit(cast(cpConstraint *)joint, &klass, a, b);
	
	joint.grv_a = groove_a;
	joint.grv_b = groove_b;
	joint.grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a)));
	joint.anchr2 = anchr2;
	
	joint.jAcc = cpvzero;
	
	return joint;
}

cpConstraint *
cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2)
{
	return cast(cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2);
}

void
cpGrooveJointSetGrooveA(cpConstraint *constraint, cpVect value)
{
	cpGrooveJoint *g = cast(cpGrooveJoint *)constraint;
	//TODO:
	//cpConstraintCheckCast(constraint, cpGrooveJoint);
	
	g.grv_a = value;
	g.grv_n = cpvperp(cpvnormalize(cpvsub(g.grv_b, value)));
	
	cpConstraintActivateBodies(constraint);
}

void
cpGrooveJointSetGrooveB(cpConstraint *constraint, cpVect value)
{
	cpGrooveJoint *g = cast(cpGrooveJoint *)constraint;
	//TODO:
	//cpConstraintCheckCast(constraint, cpGrooveJoint);
	
	g.grv_b = value;
	g.grv_n = cpvperp(cpvnormalize(cpvsub(value, g.grv_a)));
	
	cpConstraintActivateBodies(constraint);
}