// written in the D programming language

module chipmunkd.constraints.util;

import chipmunkd.cpBody;
import chipmunkd.chipmunk_types_h;
import chipmunkd.cpVect,chipmunkd.cpVect_h;

template CP_DefineClassGetter(string t)
{
	enum CP_DefineClassGetter = 
		"cpConstraintClass* "~t~"GetClass(){return cast(cpConstraintClass *)&klass;}";
}

template J_MAX(string constraint,string dt)
{
	enum J_MAX = "((cast(cpConstraint *)"~constraint~").maxForce*("~dt~"))";
}

template CONSTRAINT_BEGIN(string constraint,string a_var,string b_var)
{
	enum CONSTRAINT_BEGIN = "cpBody *"~a_var~"; cpBody *"~b_var~";
		"~a_var~" = (cast(cpConstraint *)"~constraint~")."~a_var~"; 
		"~b_var~" = (cast(cpConstraint *)"~constraint~")."~b_var~"; 
		if(
			(cpBodyIsSleeping("~a_var~") || cpBodyIsStatic("~a_var~")) && 
			(cpBodyIsSleeping("~b_var~") || cpBodyIsStatic("~b_var~")) 
		) return;
		";
}

static cpVect
relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){
	cpVect v1_sum = cpvadd(a.v, cpvmult(cpvperp(r1), a.w));
	cpVect v2_sum = cpvadd(b.v, cpvmult(cpvperp(r2), b.w));
	
	return cpvsub(v2_sum, v1_sum);
}

static cpFloat
normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){
	return cpvdot(relative_velocity(a, b, r1, r2), n);
}

static void
apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
{
	cpBodyApplyImpulse(a, cpvneg(j), r1);
	cpBodyApplyImpulse(b, j, r2);
}

static void
apply_bias_impulse(cpBody *_body, cpVect j, cpVect r)
{
	_body.v_bias = cpvadd(_body.v_bias, cpvmult(j, _body.m_inv));
	_body.w_bias += _body.i_inv*cpvcross(r, j);
}

static void
apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j)
{
	apply_bias_impulse(a, cpvneg(j), r1);
	apply_bias_impulse(b, j, r2);
}

static cpVect
clamp_vect(cpVect v, cpFloat len)
{
	return cpvclamp(v, len);
//	return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v;
}

static cpFloat
k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n)
{
	cpFloat mass_sum = a.m_inv + b.m_inv;
	cpFloat r1cn = cpvcross(r1, n);
	cpFloat r2cn = cpvcross(r2, n);
	
	cpFloat value = mass_sum + a.i_inv*r1cn*r1cn + b.i_inv*r2cn*r2cn;
	assert(value != 0.0, "Unsolvable collision or constraint.");
	
	return value;
}

static void
k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2)
{
	// calculate mass matrix
	// If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross...
	cpFloat k11, k12, k21, k22;
	cpFloat m_sum = a.m_inv + b.m_inv;
	
	// start with I*m_sum
	k11 = m_sum; k12 = 0.0f;
	k21 = 0.0f;  k22 = m_sum;
	
	// add the influence from r1
	cpFloat a_i_inv = a.i_inv;
	cpFloat r1xsq =  r1.x * r1.x * a_i_inv;
	cpFloat r1ysq =  r1.y * r1.y * a_i_inv;
	cpFloat r1nxy = -r1.x * r1.y * a_i_inv;
	k11 += r1ysq; k12 += r1nxy;
	k21 += r1nxy; k22 += r1xsq;
	
	// add the influnce from r2
	cpFloat b_i_inv = b.i_inv;
	cpFloat r2xsq =  r2.x * r2.x * b_i_inv;
	cpFloat r2ysq =  r2.y * r2.y * b_i_inv;
	cpFloat r2nxy = -r2.x * r2.y * b_i_inv;
	k11 += r2ysq; k12 += r2nxy;
	k21 += r2nxy; k22 += r2xsq;
	
	// invert
	cpFloat determinant = k11*k22 - k12*k21;
	assert(determinant != 0.0, "Unsolvable constraint.");
	
	cpFloat det_inv = 1.0f/determinant;
	*k1 = cpv( k22*det_inv, -k12*det_inv);
	*k2 = cpv(-k21*det_inv,  k11*det_inv);
}

static cpVect
mult_k(cpVect vr, cpVect k1, cpVect k2)
{
	return cpv(cpvdot(vr, k1), cpvdot(vr, k2));
}