// written in the D programming language

module chipmunkd.cpSpace;

import chipmunkd.chipmunk;

// Number of frames that contact information should persist.
//extern cpTimestamp cp_contact_persistence;

// User collision handler function types.
alias cpBool	function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionBeginFunc;
alias cpBool	function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPreSolveFunc;
alias void		function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionPostSolveFunc;
alias void		function(cpArbiter *arb, cpSpace *space, void *data)cpCollisionSeparateFunc;

// Structure for holding collision pair function information.
// Used internally.
struct cpCollisionHandler {
	cpCollisionType a;
	cpCollisionType b;
	cpCollisionBeginFunc begin;
	cpCollisionPreSolveFunc preSolve;
	cpCollisionPostSolveFunc postSolve;
	cpCollisionSeparateFunc separate;
	void *data;
}

struct cpContactBufferHeader {
	cpTimestamp stamp;
	cpContactBufferHeader *next;
	uint numContacts;
}

struct cpSpace{
	// *** User definable fields
	
	// Number of iterations to use in the impulse solver to solve contacts.
	int iterations;
	
	// Number of iterations to use in the impulse solver to solve elastic collisions.
	int elasticIterations;
	
	// Default gravity to supply when integrating rigid body motions.
	cpVect gravity;
	
	// Default damping to supply when integrating rigid body motions.
	cpFloat damping;
	
	// Speed threshold for a body to be considered idle.
	// The default value of 0 means to let the space guess a good threshold based on gravity.
	cpFloat idleSpeedThreshold;
	
	// Time a group of bodies must remain idle in order to fall asleep
	// The default value of INFINITY disables the sleeping algorithm.
	cpFloat sleepTimeThreshold;
	
	// *** Internally Used Fields
	
	// When the space is locked, you should not add or remove objects;
	cpBool locked;
	
	// Time stamp. Is incremented on every call to cpSpaceStep().
	cpTimestamp stamp;

	// The static and active shape spatial hashes.
	cpSpaceHash* staticShapes;
	cpSpaceHash* activeShapes;
	
	// List of bodies in the system.
	cpArray *bodies;
	
	// List of groups of sleeping bodies.
	cpArray *sleepingComponents;
	
	// List of active arbiters for the impulse solver.
	cpArray* arbiters, pooledArbiters;
	
	// Linked list ring of contact buffers.
	// Head is the newest buffer, and each buffer points to a newer buffer.
	// Head wraps around and points to the oldest (tail) buffer.
	cpContactBufferHeader* contactBuffersHead;
	deprecated cpContactBufferHeader* _contactBuffersTail_Deprecated;
	
	// List of buffers to be free()ed when destroying the space.
	cpArray *allocatedBuffers;
	
	// Persistant contact set.
	cpHashSet *contactSet;
	
	// List of constraints in the system.
	cpArray *constraints;
	
	// Set of collisionpair functions.
	cpHashSet *collFuncSet;
	// Default collision handler.
	cpCollisionHandler defaultHandler;
	
	cpHashSet *postStepCallbacks;
	
	cpBody staticBody;
}

//// Basic allocation/destruction functions.
//cpSpace* cpSpaceAlloc(void);
//cpSpace* cpSpaceInit(cpSpace *space);
//cpSpace* cpSpaceNew(void);
//
//void cpSpaceDestroy(cpSpace *space);
//void cpSpaceFree(cpSpace *space);
//
//// Convenience function. Frees all referenced entities. (bodies, shapes and constraints)
//void cpSpaceFreeChildren(cpSpace *space);
//
//// Collision handler management functions.
//void cpSpaceSetDefaultCollisionHandler(
//	cpSpace *space,
//	cpCollisionBeginFunc begin,
//	cpCollisionPreSolveFunc preSolve,
//	cpCollisionPostSolveFunc postSolve,
//	cpCollisionSeparateFunc separate,
//	void *data
//);
//void cpSpaceAddCollisionHandler(
//	cpSpace *space,
//	cpCollisionType a, cpCollisionType b,
//	cpCollisionBeginFunc begin,
//	cpCollisionPreSolveFunc preSolve,
//	cpCollisionPostSolveFunc postSolve,
//	cpCollisionSeparateFunc separate,
//	void *data
//);
//void cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b);
//
//// Add and remove entities from the system.
//cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape);
//cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape);
//cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body);
//cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint);
//
//void cpSpaceRemoveShape(cpSpace *space, cpShape *shape);
//void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape);
//void cpSpaceRemoveBody(cpSpace *space, cpBody *body);
//void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint);
//
//// Post Step function definition
alias void function(cpSpace *space, void *obj, void *data) cpPostStepFunc;
//// Register a post step function to be called after cpSpaceStep() has finished.
//// obj is used a key, you can only register one callback per unique value for obj
//void cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *obj, void *data);
//
//// Point query callback function
alias void function(cpShape *shape, void *data) cpSpacePointQueryFunc;
//void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data);
//cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group);
//
//// Segment query callback function
alias void function(cpShape *shape, cpFloat t, cpVect n, void *data)cpSpaceSegmentQueryFunc;
//void cpSpaceSegmentQuery(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSpaceSegmentQueryFunc func, void *data);
//cpShape *cpSpaceSegmentQueryFirst(cpSpace *space, cpVect start, cpVect end, cpLayers layers, cpGroup group, cpSegmentQueryInfo *out);
//
//// BB query callback function
alias void function(cpShape *shape, void *data)cpSpaceBBQueryFunc;
//void cpSpaceBBQuery(cpSpace *space, cpBB bb, cpLayers layers, cpGroup group, cpSpaceBBQueryFunc func, void *data);

// Shape query callback function
alias void function(cpShape *shape, cpContactPointSet *points, void *data)cpSpaceShapeQueryFunc;
//cpBool cpSpaceShapeQuery(cpSpace *space, cpShape *shape, cpSpaceShapeQueryFunc func, void *data);


//void cpSpaceActivateShapesTouchingShape(cpSpace *space, cpShape *shape);

//// Iterator function for iterating the bodies in a space.
alias void function(cpBody *_body, void *data)cpSpaceBodyIterator;
//void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data);
//
//// Spatial hash management functions.
//void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count);
//void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count);
//void cpSpaceRehashStatic(cpSpace *space);
//
//void cpSpaceRehashShape(cpSpace *space, cpShape *shape);
//
//// Update the space.
//void cpSpaceStep(cpSpace *space, cpFloat dt);


cpTimestamp cp_contact_persistence = 3;

// Equal function for contactSet.
static cpBool
contactSetEql(cpShape **shapes, cpArbiter *arb)
{
	cpShape *a = shapes[0];
	cpShape *b = shapes[1];
	
	return ((a == arb.a && b == arb.b) || (b == arb.a && a == arb.b));
}

// Transformation function for contactSet.
static void *
contactSetTrans(cpShape **shapes, cpSpace *space)
{
	if(space.pooledArbiters.num == 0){
		// arbiter pool is exhausted, make more
		int count = CP_BUFFER_BYTES/cpArbiter.sizeof;
		assert(count, "Buffer size too small.");
		
		cpArbiter *buffer = cast(cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
		cpArrayPush(space.allocatedBuffers, buffer);
		
		for(int i=0; i<count; i++) cpArrayPush(space.pooledArbiters, buffer + i);
	}
	
	return cpArbiterInit(cast(cpArbiter *) cpArrayPop(space.pooledArbiters), shapes[0], shapes[1]);
}

// Equals function for collFuncSet.
static cpBool
collFuncSetEql(cpCollisionHandler *check, cpCollisionHandler *pair)
{
	return ((check.a == pair.a && check.b == pair.b) || (check.b == pair.a && check.a == pair.b));
}

// Transformation function for collFuncSet.
static void *
collFuncSetTrans(cpCollisionHandler *handler, void *unused)
{
	cpCollisionHandler *copy = cast(cpCollisionHandler *)cpmalloc(cpCollisionHandler.sizeof);
	(*copy) = (*handler);
	
	return copy;
}

// Default collision functions.
static cpBool alwaysCollide(cpArbiter *arb, cpSpace *space, void *data){return 1;}
static void nothing(cpArbiter *arb, cpSpace *space, void *data){}

// BBfunc callback for the spatial hash.
static cpBB shapeBBFunc(cpShape *shape){return shape.bb;}

// Iterator functions for destructors.
static void             freeWrap(void         *ptr, void *unused){            cpfree(ptr);}
static void        shapeFreeWrap(cpShape      *ptr, void *unused){     cpShapeFree(ptr);}
static void         bodyFreeWrap(cpBody       *ptr, void *unused){      cpBodyFree(ptr);}
static void   constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFree(ptr);}

cpSpace *
cpSpaceAlloc()
{
	return cast(cpSpace *)cpcalloc(1, cpSpace.sizeof);
}

enum DEFAULT_DIM_SIZE				= 100.0f;
enum DEFAULT_COUNT					= 1000;
enum DEFAULT_ITERATIONS				= 10;
enum DEFAULT_ELASTIC_ITERATIONS		= 0;

cpCollisionHandler defaultHandler = {0, 0, &alwaysCollide, &alwaysCollide, &nothing, &nothing, null};

cpSpace*
cpSpaceInit(cpSpace *space)
{
	space.iterations = DEFAULT_ITERATIONS;
	space.elasticIterations = DEFAULT_ELASTIC_ITERATIONS;
//	space.sleepTicks = 300;
	
	space.gravity = cpvzero;
	space.damping = 1.0f;
	
	space.locked = 0;
	space.stamp = 0;

	space.staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc);
	space.activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, cast(cpSpaceHashBBFunc)&shapeBBFunc);
	
	space.allocatedBuffers = cpArrayNew(0);
	
	space.bodies = cpArrayNew(0);
	space.sleepingComponents = cpArrayNew(0);
	space.sleepTimeThreshold = INFINITY;
	space.idleSpeedThreshold = 0.0f;
	
	space.arbiters = cpArrayNew(0);
	space.pooledArbiters = cpArrayNew(0);
	
	space.contactBuffersHead = null;
	space.contactSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&contactSetEql, cast(cpHashSetTransFunc)&contactSetTrans);
	
	space.constraints = cpArrayNew(0);
	
	space.defaultHandler = defaultHandler;
	space.collFuncSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&collFuncSetEql, cast(cpHashSetTransFunc)&collFuncSetTrans);
	space.collFuncSet.default_value = &space.defaultHandler;
	
	space.postStepCallbacks = null;
	
	cpBodyInit(&space.staticBody, INFINITY, INFINITY);
	space.staticBody.space = space;
	
	return space;
}

cpSpace*
cpSpaceNew()
{
	return cpSpaceInit(cpSpaceAlloc());
}

void
cpSpaceDestroy(cpSpace *space)
{
	cpSpaceHashFree(space.staticShapes);
	cpSpaceHashFree(space.activeShapes);
	
	cpArrayFree(space.bodies);
	cpArrayFree(space.sleepingComponents);
	
	cpArrayFree(space.constraints);
	
	cpHashSetFree(space.contactSet);
	
	cpArrayFree(space.arbiters);
	cpArrayFree(space.pooledArbiters);
	
	if(space.allocatedBuffers){
		cpArrayEach(space.allocatedBuffers, &freeWrap, null);
		cpArrayFree(space.allocatedBuffers);
	}
	
	if(space.postStepCallbacks){
		cpHashSetEach(space.postStepCallbacks, &freeWrap, null);
		cpHashSetFree(space.postStepCallbacks);
	}
	
	if(space.collFuncSet){
		cpHashSetEach(space.collFuncSet, &freeWrap, null);
		cpHashSetFree(space.collFuncSet);
	}
}

void
cpSpaceFree(cpSpace *space)
{
	if(space){
		cpSpaceDestroy(space);
		cpfree(space);
	}
}

void
cpSpaceFreeChildren(cpSpace *space)
{
	cpArray *components = space.sleepingComponents;
	while(components.num) cpBodyActivate(cast(cpBody *)components.arr[0]);
	
	cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null);
	cpSpaceHashEach(space.activeShapes, cast(cpSpaceHashIterator)&shapeFreeWrap, null);
	cpArrayEach(space.bodies,           cast(cpArrayIter)&bodyFreeWrap,          null);
	cpArrayEach(space.constraints,      cast(cpArrayIter)&constraintFreeWrap,    null);
}

void
cpSpaceAddCollisionHandler(
	cpSpace *space,
	cpCollisionType a, cpCollisionType b,
	cpCollisionBeginFunc begin,
	cpCollisionPreSolveFunc preSolve,
	cpCollisionPostSolveFunc postSolve,
	cpCollisionSeparateFunc separate,
	void *data
){
	// Remove any old function so the new one will get added.
	cpSpaceRemoveCollisionHandler(space, a, b);
	
	cpCollisionHandler handler = {
		a, b,
		begin ? begin : &alwaysCollide,
		preSolve ? preSolve : &alwaysCollide,
		postSolve ? postSolve : &nothing,
		separate ? separate : &nothing,
		data
	};
	
	cpHashSetInsert(space.collFuncSet, CP_HASH_PAIR(a, b), &handler, null);
}

void
cpSpaceRemoveCollisionHandler(cpSpace *space, cpCollisionType a, cpCollisionType b)
{
	struct tmp{cpCollisionType a, b;} tmp ids = {a, b};
	cpCollisionHandler *old_handler = cast(cpCollisionHandler *) cpHashSetRemove(space.collFuncSet, CP_HASH_PAIR(a, b), &ids);
	cpfree(old_handler);
}

void
cpSpaceSetDefaultCollisionHandler(
	cpSpace *space,
	cpCollisionBeginFunc begin,
	cpCollisionPreSolveFunc preSolve,
	cpCollisionPostSolveFunc postSolve,
	cpCollisionSeparateFunc separate,
	void *data
){
	cpCollisionHandler handler = {
		0, 0,
		begin ? begin : &alwaysCollide,
		preSolve ? preSolve : &alwaysCollide,
		postSolve ? postSolve : &nothing,
		separate ? separate : &nothing,
		data
	};
	
	space.defaultHandler = handler;
}

//#pragma mark Body, Shape, and Joint Management

void cpAssertSpaceUnlocked(cpSpace* _space){
	assert(!_space.locked,
		"This addition/removal cannot be done safely during a call to cpSpaceStep(). "
		"Put these calls into a Post Step Callback."
	);}

static void
cpBodyAddShape(cpBody *_body, cpShape *shape)
{
	shape.next = shape._body.shapesList;
	shape._body.shapesList = shape;
}

static void
cpBodyRemoveShape(cpBody *_body, cpShape *shape)
{
	cpShape **prev_ptr = &_body.shapesList;
	cpShape *node = _body.shapesList;
	
	while(node && node != shape){
		prev_ptr = &node.next;
		node = node.next;
	}
	
	assert(node, "Attempted to remove a shape from a body it was never attached to.");
	(*prev_ptr) = node.next;
}

cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
	cpBody *_body = shape._body;
	if(!_body || _body == &space.staticBody) return cpSpaceAddStaticShape(space, shape);
	
	assert(!cpHashSetFind(space.activeShapes.handleSet, shape.hashid, shape),
		"Cannot add the same shape more than once.");
	cpAssertSpaceUnlocked(space);
	
	cpBodyActivate(_body);
	cpBodyAddShape(_body, shape);
	
	cpShapeCacheBB(shape);
	cpSpaceHashInsert(space.activeShapes, shape, shape.hashid, shape.bb);
		
	return shape;
}

cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
	assert(!cpHashSetFind(space.staticShapes.handleSet, shape.hashid, shape),
		"Cannot add the same static shape more than once.");
	cpAssertSpaceUnlocked(space);
	
	if(!shape._body) shape._body = &space.staticBody;
	
	cpShapeCacheBB(shape);
	cpSpaceActivateShapesTouchingShape(space, shape);
	cpSpaceHashInsert(space.staticShapes, shape, shape.hashid, shape.bb);
	
	return shape;
}

cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *_body)
{
	mixin(cpAssertWarn!("!cpBodyIsStatic(_body)", "Static bodies cannot be added to a space as they are not meant to be simulated.",__FILE__,__LINE__));
	assert(!_body.space, "Cannot add a body to a more than one space or to the same space twice.");
//	cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
	
	cpArrayPush(space.bodies, _body);
	_body.space = space;
	
	return _body;
}

cpConstraint *
cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
{
	assert(!cpArrayContains(space.constraints, constraint), "Cannot add the same constraint more than once.");
//	cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback.
	
	if(!constraint.a) constraint.a = &space.staticBody;
	if(!constraint.b) constraint.b = &space.staticBody;
	
	cpBodyActivate(constraint.a);
	cpBodyActivate(constraint.b);
	cpArrayPush(space.constraints, constraint);
	
	return constraint;
}

struct removalContext {
	cpSpace *space;
	cpShape *shape;
}

// Hashset filter func to throw away old arbiters.
static cpBool
contactSetFilterRemovedShape(cpArbiter *arb, removalContext *context)
{
	if(context.shape == arb.a || context.shape == arb.b){
		if(arb.state != cpArbiterState.cpArbiterStateCached){
			arb.handler.separate(arb, context.space, arb.handler.data);
		}
		
		cpArrayPush(context.space.pooledArbiters, arb);
		return cpFalse;
	}
	
	return cpTrue;
}

void
cpSpaceRemoveShape(cpSpace *space, cpShape *shape)
{
	cpBody *_body = shape._body;
	if(cpBodyIsStatic(_body)){
		cpSpaceRemoveStaticShape(space, shape);
		return;
	}

	cpBodyActivate(_body);
	
	cpAssertSpaceUnlocked(space);
	mixin(cpAssertWarn!("cpHashSetFind(space.activeShapes.handleSet, shape.hashid, shape) !is null",
		"Cannot remove a shape that was never added to the space. (Removed twice maybe?)",__FILE__,__LINE__));
	
	cpBodyRemoveShape(_body, shape);
	
	removalContext context = {space, shape};
	cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context);
	cpSpaceHashRemove(space.activeShapes, shape, shape.hashid);
}

void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
	mixin(cpAssertWarn!("cpHashSetFind(space.staticShapes.handleSet, shape.hashid, shape) !is null",
		"Cannot remove a static or sleeping shape that was never added to the space. (Removed twice maybe?)",__FILE__,__LINE__));
	cpAssertSpaceUnlocked(space);
	
	removalContext context = {space, shape};
	cpHashSetFilter(space.contactSet, cast(cpHashSetFilterFunc)&contactSetFilterRemovedShape, &context);
	cpSpaceHashRemove(space.staticShapes, shape, shape.hashid);
	
	cpSpaceActivateShapesTouchingShape(space, shape);
}

void
cpSpaceRemoveBody(cpSpace *space, cpBody *_body)
{
	mixin(cpAssertWarn!("_body.space == space",
		"Cannot remove a body that was never added to the space. (Removed twice maybe?)"));
	cpAssertSpaceUnlocked(space);
	
	cpBodyActivate(_body);
	cpArrayDeleteObj(space.bodies, _body);
	_body.space = null;
}

void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
	mixin(cpAssertWarn!("cpArrayContains(space.constraints, constraint)",
		"Cannot remove a constraint that was never added to the space. (Removed twice maybe?)",__FILE__,__LINE__));
//	cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
	
	cpBodyActivate(constraint.a);
	cpBodyActivate(constraint.b);
	cpArrayDeleteObj(space.constraints, constraint);
}

//#pragma mark Spatial Hash Management

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

void
cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count)
{
	cpSpaceHashResize(space.staticShapes, dim, count);
	cpSpaceHashRehash(space.staticShapes);
}

void
cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count)
{
	cpSpaceHashResize(space.activeShapes, dim, count);
}

void 
cpSpaceRehashStatic(cpSpace *space)
{
	cpSpaceHashEach(space.staticShapes, cast(cpSpaceHashIterator)&updateBBCache, null);
	cpSpaceHashRehash(space.staticShapes);
}

void
cpSpaceRehashShape(cpSpace *space, cpShape *shape)
{
	cpShapeCacheBB(shape);
	
	// attempt to rehash the shape in both hashes
	cpSpaceHashRehashObject(space.activeShapes, shape, shape.hashid);
	cpSpaceHashRehashObject(space.staticShapes, shape, shape.hashid);
}