// written in the D programming language

module chipmunkd.cpSpaceStep;

import chipmunkd.chipmunk;
//import chipmunkd.chipmunk_types_h;
//import chipmunkd.cpVect,chipmunkd.cpVect_h;
//import chipmunkd.cpHashSet;
//import chipmunkd.cpCollision;
//import chipmunkd.cpBody;
//import chipmunkd.cpArray;
//import chipmunkd.cpShape;
//import chipmunkd.cpBB;
//import chipmunkd.cpArbiter;
//import chipmunkd.cpSpaceHash;
//import chipmunkd.constraints.cpConstraint;
//import chipmunkd.cpSpaceQuery;
//
//#pragma mark Post Step Callback Functions

struct postStepCallback {
	cpPostStepFunc func;
	void *obj;
	void *data;
}

static cpBool
postStepFuncSetEql(postStepCallback *a, postStepCallback *b){
	return a.obj == b.obj;
}

static void *
postStepFuncSetTrans(postStepCallback *callback, void *ignored)
{
	postStepCallback *value = cast(postStepCallback *)cpmalloc(postStepCallback.sizeof);
	(*value) = (*callback);
	
	return value;
}

void
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data)
{
	if(!space.postStepCallbacks){
		space.postStepCallbacks = cpHashSetNew(0, cast(cpHashSetEqlFunc)&postStepFuncSetEql, cast(cpHashSetTransFunc)&postStepFuncSetTrans);
	}
	
	postStepCallback callback = {func, obj, data};
	cpHashSetInsert(space.postStepCallbacks, cast(cpHashValue)cast(size_t)obj, &callback, null);
}

//#pragma mark Contact Buffer Functions

enum CP_CONTACTS_BUFFER_SIZE = ((CP_BUFFER_BYTES - cpContactBufferHeader.sizeof)/cpContact.sizeof);

struct cpContactBuffer {
	cpContactBufferHeader header;
	cpContact[CP_CONTACTS_BUFFER_SIZE] contacts;
}

static cpContactBufferHeader *
cpSpaceAllocContactBuffer(cpSpace *space)
{
	cpContactBuffer *buffer = cast(cpContactBuffer *)cpmalloc(cpContactBuffer.sizeof);
	cpArrayPush(space.allocatedBuffers, buffer);
	return cast(cpContactBufferHeader *)buffer;
}

static cpContactBufferHeader *
cpContactBufferHeaderInit(cpContactBufferHeader *header, cpTimestamp stamp, cpContactBufferHeader *splice)
{
	header.stamp = stamp;
	header.next = (splice ? splice.next : header);
	header.numContacts = 0;
	
	return header;
}

static void
cpSpacePushFreshContactBuffer(cpSpace *space)
{
	cpTimestamp stamp = space.stamp;
	
	cpContactBufferHeader *head = space.contactBuffersHead;
	
	if(!head){
		// No buffers have been allocated, make one
		space.contactBuffersHead = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, null);
	} else if(stamp - head.next.stamp > cp_contact_persistence){
		// The tail buffer is available, rotate the ring
	cpContactBufferHeader *tail = head.next;
		space.contactBuffersHead = cpContactBufferHeaderInit(tail, stamp, tail);
	} else {
		// Allocate a new buffer and push it into the ring
		cpContactBufferHeader *buffer = cpContactBufferHeaderInit(cpSpaceAllocContactBuffer(space), stamp, head);
		space.contactBuffersHead = head.next = buffer;
	}
}


static cpContact *
cpContactBufferGetArray(cpSpace *space)
{
	if(space.contactBuffersHead.numContacts + CP_MAX_CONTACTS_PER_ARBITER > CP_CONTACTS_BUFFER_SIZE){
		// contact buffer could overflow on the next collision, push a fresh one.
		cpSpacePushFreshContactBuffer(space);
	}
	
	cpContactBufferHeader *head = space.contactBuffersHead;
	return &(cast(cpContactBuffer *)head).contacts[head.numContacts];
}

static void
cpSpacePushContacts(cpSpace *space, int count){
	assert(count <= CP_MAX_CONTACTS_PER_ARBITER, "Internal error, too many contact point overflow!");
	space.contactBuffersHead.numContacts += count;
}

static void
cpSpacePopContacts(cpSpace *space, int count){
	space.contactBuffersHead.numContacts -= count;
}

//#pragma mark Collision Detection Functions

static cpBool
queryReject(cpShape *a, cpShape *b)
{
	return
		// BBoxes must overlap
		!cpBBintersects(a.bb, b.bb)
		// Don't collide shapes attached to the same body.
		|| a._body == b._body
		// Don't collide objects in the same non-zero group
		|| (a.group && a.group == b.group)
		// Don't collide objects that don't share at least on layer.
		|| !(a.layers & b.layers);
}

// Callback from the spatial hash.
static void
queryFunc(cpShape *a, cpShape *b, cpSpace *space)
{
	// Reject any of the simple cases
	if(queryReject(a,b)) return;
	
	// Find the collision pair function for the shapes.
	struct tmp{cpCollisionType a, b;} tmp ids = {a.collision_type, b.collision_type};
	cpHashValue collHashID = CP_HASH_PAIR(a.collision_type, b.collision_type);
	cpCollisionHandler *handler = cast(cpCollisionHandler *)cpHashSetFind(space.collFuncSet, collHashID, &ids);
	
	cpBool sensor = a.sensor || b.sensor;
	if(sensor && handler == &space.defaultHandler) return;
	
	// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
	if(a.klass.type > b.klass.type){
		cpShape *temp = a;
		a = b;
		b = temp;
	}
	
	// Narrow-phase collision detection.
	cpContact *contacts = cpContactBufferGetArray(space);
	int numContacts = cpCollideShapes(a, b, contacts);
	if(!numContacts) return; // Shapes are not colliding.
	cpSpacePushContacts(space, numContacts);
	
	// Get an arbiter from space.contactSet for the two shapes.
	// This is where the persistant contact magic comes from.
	cpShape *shape_pair[] = [a, b];
	cpHashValue arbHashID = CP_HASH_PAIR(cast(size_t)a, cast(size_t)b);
	cpArbiter *arb = cast(cpArbiter *)cpHashSetInsert(space.contactSet, arbHashID, shape_pair.ptr, space);
	cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array
	
	// Call the begin function first if it's the first step
	if(arb.state == cpArbiterState.cpArbiterStateFirstColl && !handler.begin(arb, space, handler.data)){
		cpArbiterIgnore(arb); // permanently ignore the collision until separation
	}
	
	if(
		// Ignore the arbiter if it has been flagged
		(arb.state != cpArbiterState.cpArbiterStateIgnore) && 
		// Call preSolve
		handler.preSolve(arb, space, handler.data) &&
		// Process, but don't add collisions for sensors.
		!sensor
	){
		cpArrayPush(space.arbiters, arb);
	} else {
		cpSpacePopContacts(space, numContacts);
		
		arb.contacts = null;
		arb.numContacts = 0;
		
		// Normally arbiters are set as used after calling the post-step callback.
		// However, post-step callbacks are not called for sensors or arbiters rejected from pre-solve.
		if(arb.state != cpArbiterState.cpArbiterStateIgnore) arb.state = cpArbiterState.cpArbiterStateNormal;
	}
	
	// Time stamp the arbiter so we know it was used recently.
	arb.stamp = space.stamp;
}

// Iterator for active/static hash collisions.
static void
active2staticIter(cpShape *shape, cpSpace *space)
{
	cpSpaceHashQuery(space.staticShapes, shape, shape.bb, cast(cpSpaceHashQueryFunc)&queryFunc, space);
}

// Hashset filter func to throw away old arbiters.
static cpBool
contactSetFilter(cpArbiter *arb, cpSpace *space)
{
	if(space.sleepTimeThreshold != INFINITY){
		cpBody *a = arb.a._body;
		cpBody *b = arb.b._body;
		
		// both bodies are either static or sleeping
		cpBool sleepingNow =
			(cpBodyIsStatic(a) || cpBodyIsSleeping(a)) &&
			(cpBodyIsStatic(b) || cpBodyIsSleeping(b));
		
		if(sleepingNow){
			arb.state = cpArbiterState.cpArbiterStateSleep;
			return cpTrue;
		} else if(arb.state == cpArbiterState.cpArbiterStateSleep){
			// wake up the arbiter and continue as normal
			arb.state = cpArbiterState.cpArbiterStateNormal;
			// TODO is it possible that cpArbiterStateIgnore should be set here instead?
		}
	}
	
	cpTimestamp ticks = space.stamp - arb.stamp;
	
	// was used last frame, but not this one
	if(ticks >= 1 && arb.state != cpArbiterState.cpArbiterStateCached){
		arb.handler.separate(arb, space, arb.handler.data);
		arb.state = cpArbiterState.cpArbiterStateCached;
	}
	
	if(ticks >= cp_contact_persistence){
		arb.contacts = null;
		arb.numContacts = 0;
		
		cpArrayPush(space.pooledArbiters, arb);
		return cpFalse;
	}
	
	return cpTrue;
}

// Hashset filter func to call and throw away post step callbacks.
static void
postStepCallbackSetIter(postStepCallback *callback, cpSpace *space)
{
	callback.func(space, callback.obj, callback.data);
	cpfree(callback);
}

//#pragma mark All Important cpSpaceStep() Function

//void cpSpaceProcessComponents(cpSpace *space, cpFloat dt);

static void updateBBCache(cpShape *shape, void *unused){cpShapeCacheBB(shape);}

void
cpSpaceStep(cpSpace *space, cpFloat dt)
{
	if(!dt) return; // don't step if the timestep is 0!
	
	cpFloat dt_inv = 1.0f/dt;

	cpArray *bodies = space.bodies;
	cpArray *constraints = space.constraints;
	
	// Empty the arbiter list.
	space.arbiters.num = 0;

	// Integrate positions.
	for(int i=0; i<bodies.num; i++){
		cpBody *_body = cast(cpBody *)bodies.arr[i];
		_body.position_func(_body, dt);
	}
	
	// Pre-cache BBoxes and shape data.
	cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&updateBBCache, null);
	
	space.locked = cpTrue;
	
	// Collide!
	cpSpacePushFreshContactBuffer(space);
	if(space.staticShapes.handleSet.entries)
		cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&active2staticIter, space);
	cpSpaceHashQueryRehash(space.activeShapes, cast(cpSpaceHashQueryFunc)&queryFunc, space);
	
	space.locked = cpFalse;
	
	// If body sleeping is enabled, do that now.
	if(space.sleepTimeThreshold != INFINITY){
		cpSpaceProcessComponents(space, dt);
		bodies = space.bodies; // rebuilt by processContactComponents()
	}
	
	// Clear out old cached arbiters and dispatch untouch functions
	cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilter, space);

	// Prestep the arbiters.
	cpArray *arbiters = space.arbiters;
	for(int i=0; i<arbiters.num; i++)
		cpArbiterPreStep(cast(cpArbiter *)arbiters.arr[i], dt_inv);

	// Prestep the constraints.
	for(int i=0; i<constraints.num; i++){
		cpConstraint *constraint = cast(cpConstraint *)constraints.arr[i];
		constraint.klass.preStep(constraint, dt, dt_inv);
	}

	for(int i=0; i<space.elasticIterations; i++){
		for(int j=0; j<arbiters.num; j++)
			cpArbiterApplyImpulse(cast(cpArbiter *)arbiters.arr[j], 1.0f);
			
		for(int j=0; j<constraints.num; j++){
			cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
			constraint.klass.applyImpulse(constraint);
		}
	}

	// Integrate velocities.
	cpFloat damping = cpfpow(1.0f/space.damping, -dt);
	for(int i=0; i<bodies.num; i++){
		cpBody *_body = cast(cpBody *)bodies.arr[i];
		_body.velocity_func(_body, space.gravity, damping, dt);
	}

	for(int i=0; i<arbiters.num; i++)
		cpArbiterApplyCachedImpulse(cast(cpArbiter *)arbiters.arr[i]);
	
	// run the old-style elastic solver if elastic iterations are disabled
	cpFloat elasticCoef = (space.elasticIterations ? 0.0f : 1.0f);
	
	// Run the impulse solver.
	for(int i=0; i<space.iterations; i++){
		for(int j=0; j<arbiters.num; j++)
			cpArbiterApplyImpulse(cast(cpArbiter *)arbiters.arr[j], elasticCoef);
			
		for(int j=0; j<constraints.num; j++){
			cpConstraint *constraint = cast(cpConstraint *)constraints.arr[j];
			constraint.klass.applyImpulse(constraint);
		}
	}
	
	space.locked = cpTrue;
	
	// run the post solve callbacks
	for(int i=0; i<arbiters.num; i++){
		cpArbiter *arb = cast(cpArbiter *) arbiters.arr[i];
		
		cpCollisionHandler *handler = arb.handler;
		handler.postSolve(arb, space, handler.data);
		
		arb.state = cpArbiterState.cpArbiterStateNormal;
	}
	
	space.locked = cpFalse;
	
	// Run the post step callbacks
	// Loop because post step callbacks may create more post step callbacks
	while(space.postStepCallbacks){
		cpHashSet *callbacks = space.postStepCallbacks;
		space.postStepCallbacks = null;
		
		cpHashSetEach(callbacks, cast(cpHashSetIterFunc)&postStepCallbackSetIter, space);
		cpHashSetFree(callbacks);
	}
	
	// Increment the stamp.
	space.stamp++;
}