// written in the D programming language

module chipmunkd.cpArbiter;

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

// Determines how fast penetrations resolve themselves.
//extern cpFloat cp_bias_coef;
// Amount of allowed penetration. Used to reduce vibrating contacts.
//extern cpFloat cp_collision_slop;

// Data structure for contact points.
struct cpContact {
	// Contact point and normal.
	cpVect p, n;
	// Penetration distance.
	cpFloat dist;
	
	// Calculated by cpArbiterPreStep().
	cpVect r1, r2;
	cpFloat nMass, tMass, bounce;

	// Persistant contact information.
	cpFloat jnAcc, jtAcc, jBias;
	cpFloat bias;
	
	// Hash value used to (mostly) uniquely identify a contact.
	cpHashValue hash;
}

//// Contacts are always allocated in groups.
//cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash);
//
//// Sum the contact impulses. (Can be used after cpSpaceStep() returns)
//cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts);
//cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts);

enum CP_MAX_CONTACTS_PER_ARBITER = 6;

enum cpArbiterState {
	cpArbiterStateNormal,
	cpArbiterStateFirstColl,
	cpArbiterStateIgnore,
	cpArbiterStateSleep,
	cpArbiterStateCached,
}

// Data structure for tracking collisions between shapes.
struct cpArbiter {
	// Information on the contact points between the objects.
	int numContacts;
	cpContact *contacts;
	
	// The two shapes and bodies involved in the collision.
	// These variables are NOT in the order defined by the collision handler.
	// Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from
	// many headaches
	cpShape* a;
	cpShape* b;
	
	// Calculated before calling the pre-solve collision handler
	// Override them with custom values if you want specialized behavior
	cpFloat e;
	cpFloat u;
	 // Used for surface_v calculations, implementation may change
	cpVect surface_vr;
	
	// Time stamp of the arbiter. (from cpSpace)
	cpTimestamp stamp;
	
	cpCollisionHandler *handler;
	
	// Are the shapes swapped in relation to the collision handler?
	cpBool swappedColl;
	cpArbiterState state;
}

//// Arbiters are allocated in large buffers by the space and don't require a destroy function
//cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b);
//
//// These functions are all intended to be used internally.
//// Inject new contact points into the arbiter while preserving contact history.
//void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b);
//// Precalculate values used by the solver.
//void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv);
//void cpArbiterApplyCachedImpulse(cpArbiter *arb);
//// Run an iteration of the solver on the arbiter.
//void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef);
//
//// Arbiter Helper Functions
//cpVect cpArbiterTotalImpulse(cpArbiter *arb);
//cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb);
//void cpArbiterIgnore(cpArbiter *arb);


static void
cpArbiterGetShapes(/+const+/ cpArbiter *arb, cpShape **a, cpShape **b)
{
	if(arb.swappedColl){
		(*a) = arb.b, (*b) = arb.a;
	} else {
		(*a) = arb.a, (*b) = arb.b;
	}
}

template CP_ARBITER_GET_SHAPES(string arb,string a,string b)
{
	enum CP_ARBITER_GET_SHAPES = "cpShape* "~a~", "~b~";"~
		"cpArbiterGetShapes("~arb~", &"~a~", &"~b~");";
}

static void
cpArbiterGetBodies(/+const+/ cpArbiter *arb, cpBody **a, cpBody **b)
{
	//CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b);
	cpShape *shape_a, shape_b; cpArbiterGetShapes(arb, &shape_a, &shape_b);
	(*a) = shape_a._body;
	(*b) = shape_b._body;
}
//#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b);

static cpBool
cpArbiterIsFirstContact(const cpArbiter *arb)
{
	return arb.state == cpArbiterState.cpArbiterStateFirstColl;
}

static int
cpArbiterGetCount(const cpArbiter *arb)
{
	return arb.numContacts;
}

static cpVect
cpArbiterGetNormal(const cpArbiter *arb, int i)
{
	cpVect n = arb.contacts[i].n;
	return arb.swappedColl ? cpvneg(n) : n;
}

static cpVect
cpArbiterGetPoint(const cpArbiter *arb, int i)
{
	return arb.contacts[i].p;
}


static cpFloat 
cpArbiteGetDepth(const cpArbiter *arb, int i)
{
	return arb.contacts[i].dist;
}

struct cpContactPointSet {
	int count;
	
	struct TPoint
	{
		cpVect point, normal;
		cpFloat dist = 0;
	}
	
	TPoint[CP_MAX_CONTACTS_PER_ARBITER] points;
}

static cpContactPointSet
cpArbiterGetContactPointSet(const cpArbiter *arb)
{
	cpContactPointSet set;
	set.count = cpArbiterGetCount(arb);
	
	int i;
	for(i=0; i<set.count; i++){
		set.points[i].point = arb.contacts[i].p;
		set.points[i].normal = arb.contacts[i].p;
		set.points[i].dist = arb.contacts[i].dist;
	}
	
	return set;
}

// cpArbiter.c --------------------------

cpFloat cp_bias_coef = 0.1f;
cpFloat cp_collision_slop = 0.1f;

cpContact*
cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash)
{
	con.p = p;
	con.n = n;
	con.dist = dist;
	
	con.jnAcc = 0.0f;
	con.jtAcc = 0.0f;
	con.jBias = 0.0f;
	
	con.hash = hash;
		
	return con;
}

cpVect
cpArbiterTotalImpulse(cpArbiter *arb)
{
	cpContact *contacts = arb.contacts;
	cpVect sum = cpvzero;
	
	for(int i=0, count=arb.numContacts; i<count; i++){
		cpContact *con = &contacts[i];
		sum = cpvadd(sum, cpvmult(con.n, con.jnAcc));
	}
		
	return sum;
}

cpVect
cpArbiterTotalImpulseWithFriction(cpArbiter *arb)
{
	cpContact *contacts = arb.contacts;
	cpVect sum = cpvzero;
	
	for(int i=0, count=arb.numContacts; i<count; i++){
		cpContact *con = &contacts[i];
		sum = cpvadd(sum, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc)));
	}
		
	return sum;
}

cpFloat
cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts)
{
	cpFloat fsum = 0.0f;
	cpVect vsum = cpvzero;
	
	for(int i=0; i<numContacts; i++){
		cpContact *con = &contacts[i];
		cpVect j = cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc));
		
		fsum += cpvlength(j);
		vsum = cpvadd(vsum, j);
	}
	
	cpFloat vmag = cpvlength(vsum);
	return (1.0f - vmag/fsum);
}

void
cpArbiterIgnore(cpArbiter *arb)
{
	arb.state = cpArbiterState.cpArbiterStateIgnore;
}

cpArbiter*
cpArbiterAlloc()
{
	return cast(cpArbiter *)cpcalloc(1, cpArbiter.sizeof);
}

cpArbiter*
cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
{
	arb.handler = null;
	arb.swappedColl = cpFalse;
	   
	arb.e = 0.0f;
	arb.u = 0.0f;
	arb.surface_vr = cpvzero;
	
	arb.numContacts = 0;
	arb.contacts = null;
	
	arb.a = a;
	arb.b = b;
	
	arb.stamp = 0;
	arb.state = cpArbiterState.cpArbiterStateFirstColl;
	
	return arb;
}

cpArbiter*
cpArbiterNew(cpShape *a, cpShape *b)
{
	return cpArbiterInit(cpArbiterAlloc(), a, b);
}

void
cpArbiterDestroy(cpArbiter *arb)
{
//	if(arb.contacts) cpfree(arb.contacts);
}

void
cpArbiterFree(cpArbiter *arb)
{
	if(arb){
		cpArbiterDestroy(arb);
		cpfree(arb);
	}
}

void
cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
{
	// Arbiters without contact data may exist if a collision function rejected the collision.
	if(arb.contacts){
		// Iterate over the possible pairs to look for hash value matches.
		for(int i=0; i<arb.numContacts; i++){
			cpContact *old = &arb.contacts[i];
			
			for(int j=0; j<numContacts; j++){
				cpContact *new_contact = &contacts[j];
				
				// This could trigger false positives, but is fairly unlikely nor serious if it does.
				if(new_contact.hash == old.hash){
					// Copy the persistant contact information.
					new_contact.jnAcc = old.jnAcc;
					new_contact.jtAcc = old.jtAcc;
				}
			}
		}
	}
	
	arb.contacts = contacts;
	arb.numContacts = numContacts;
	
	arb.handler = handler;
	arb.swappedColl = (a.collision_type != handler.a);
	
	arb.e = a.e * b.e;
	arb.u = a.u * b.u;
	arb.surface_vr = cpvsub(a.surface_v, b.surface_v);
	
	// For collisions between two similar primitive types, the order could have been swapped.
	arb.a = a;
	arb.b = b;
	
	// mark it as new if it's been cached
	if(arb.state == cpArbiterState.cpArbiterStateCached) arb.state = cpArbiterState.cpArbiterStateFirstColl;
}

void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
	cpBody *a = arb.a._body;
	cpBody *b = arb.b._body;
	
	for(int i=0; i<arb.numContacts; i++){
		cpContact *con = &arb.contacts[i];
		
		// Calculate the offsets.
		con.r1 = cpvsub(con.p, a.p);
		con.r2 = cpvsub(con.p, b.p);
		
		// Calculate the mass normal and mass tangent.
		con.nMass = 1.0f/k_scalar(a, b, con.r1, con.r2, con.n);
		con.tMass = 1.0f/k_scalar(a, b, con.r1, con.r2, cpvperp(con.n));
				
		// Calculate the target bias velocity.
		con.bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con.dist + cp_collision_slop);
		con.jBias = 0.0f;
		
		// Calculate the target bounce velocity.
		con.bounce = normal_relative_velocity(a, b, con.r1, con.r2, con.n)*arb.e;//cpvdot(con.n, cpvsub(v2, v1))*e;
	}
}

void
cpArbiterApplyCachedImpulse(cpArbiter *arb)
{
	cpShape *shapea = arb.a;
	cpShape *shapeb = arb.b;
		
	arb.u = shapea.u * shapeb.u;
	arb.surface_vr = cpvsub(shapeb.surface_v, shapea.surface_v);

	cpBody *a = shapea._body;
	cpBody *b = shapeb._body;
	
	for(int i=0; i<arb.numContacts; i++){
		cpContact *con = &arb.contacts[i];
		apply_impulses(a, b, con.r1, con.r2, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc)));
	}
}

void
cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
{
	cpBody *a = arb.a._body;
	cpBody *b = arb.b._body;

	for(int i=0; i<arb.numContacts; i++){
		cpContact *con = &arb.contacts[i];
		cpVect n = con.n;
		cpVect r1 = con.r1;
		cpVect r2 = con.r2;
		
		// Calculate the relative bias velocities.
		cpVect vb1 = cpvadd(a.v_bias, cpvmult(cpvperp(r1), a.w_bias));
		cpVect vb2 = cpvadd(b.v_bias, cpvmult(cpvperp(r2), b.w_bias));
		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
		
		// Calculate and clamp the bias impulse.
		cpFloat jbn = (con.bias - vbn)*con.nMass;
		cpFloat jbnOld = con.jBias;
		con.jBias = cpfmax(jbnOld + jbn, 0.0f);
		jbn = con.jBias - jbnOld;
		
		// Apply the bias impulse.
		apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn));

		// Calculate the relative velocity.
		cpVect vr = relative_velocity(a, b, r1, r2);
		cpFloat vrn = cpvdot(vr, n);
		
		// Calculate and clamp the normal impulse.
		cpFloat jn = -(con.bounce*eCoef + vrn)*con.nMass;
		cpFloat jnOld = con.jnAcc;
		con.jnAcc = cpfmax(jnOld + jn, 0.0f);
		jn = con.jnAcc - jnOld;
		
		// Calculate the relative tangent velocity.
		cpFloat vrt = cpvdot(cpvadd(vr, arb.surface_vr), cpvperp(n));
		
		// Calculate and clamp the friction impulse.
		cpFloat jtMax = arb.u*con.jnAcc;
		cpFloat jt = -vrt*con.tMass;
		cpFloat jtOld = con.jtAcc;
		con.jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
		jt = con.jtAcc - jtOld;
		
		// Apply the final impulse.
		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt)));
	}
}