// Hashset filter func to call and throw away post step callbacks.
static int
postStepCallbackSetFilter(postStepCallback *callback, cpSpace *space)
{
    callback->func(space, callback->obj, callback->data);
    cpfree(callback);

    return 0;
}

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

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

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

// Frees the old table, and allocates a new one.
static void
cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells)
{
	if(hash->table)
		cpfree(hash->table);
	
	hash->numcells = numcells;
	hash->table = (cpSpaceHashBin **)cpcalloc(numcells, sizeof(cpSpaceHashBin *));
}

void
cpHashSetDestroy(cpHashSet *set)
{
	// Free the table.
	cpfree(set->table);
	
	if(set->allocatedBuffers) cpArrayEach(set->allocatedBuffers, freeWrap, NULL);
	cpArrayFree(set->allocatedBuffers);
}

// Free the recycled hash bins.
static void
freeBins(cpSpaceHash *hash)
{
	cpSpaceHashBin *bin = hash->bins;
	while(bin){
		cpSpaceHashBin *next = bin->next;
		cpfree(bin);
		bin = next;
	}
}

void
cpPolylineSetDestroy(cpPolylineSet *set, cpBool freePolylines)
{
	if(freePolylines){
		for(int i=0; i<set->count; i++){
			cpPolylineFree(set->lines[i]);
		}
	}
	
	cpfree(set->lines);
}

static void
cpSpaceHashDestroy(cpSpaceHash *hash)
{
	if(hash->table) clearTable(hash);
	cpfree(hash->table);
	
	cpHashSetFree(hash->handleSet);
	
	cpArrayFreeEach(hash->allocatedBuffers, cpfree);
	cpArrayFree(hash->allocatedBuffers);
	cpArrayFree(hash->pooledHandles);
}

void
cpSpaceHashDestroy(cpSpaceHash *hash)
{
	clearHash(hash);
	freeBins(hash);
	
	// Free the handles.
	cpHashSetEach(hash->handleSet, &handleFreeWrap, NULL);
	cpHashSetFree(hash->handleSet);
	
	cpfree(hash->table);
}

void
cpSpaceHashDestroy(cpSpaceHash *hash)
{
	clearHash(hash);
	
	cpHashSetFree(hash->handleSet);
	
	cpArrayEach(hash->allocatedBuffers, freeWrap, NULL);
	cpArrayFree(hash->allocatedBuffers);
	cpArrayFree(hash->pooledHandles);
	
	cpfree(hash->table);
}

void
cpSpaceActivateBody(cpSpace *space, cpBody *body)
{
	cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rogue body.");
		
	if(space->locked){
		// cpSpaceActivateBody() is called again once the space is unlocked
		if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body);
	} else {
		cpAssertSoft(body->node.root == NULL && body->node.next == NULL, "Internal error: Activating body non-NULL node pointers.");
		cpArrayPush(space->bodies, body);

		CP_BODY_FOREACH_SHAPE(body, shape){
			cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
			cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
		}
		
		CP_BODY_FOREACH_ARBITER(body, arb){
			cpBody *bodyA = arb->body_a;
			
			// Arbiters are shared between two bodies that are always woken up together.
			// You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter.
			// The edge case is when static bodies are involved as the static bodies never actually sleep.
			// If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked.
			if(body == bodyA || cpBodyIsStatic(bodyA)){
				int numContacts = arb->numContacts;
				cpContact *contacts = arb->contacts;
				
				// Restore contact values back to the space's contact buffer memory
				arb->contacts = cpContactBufferGetArray(space);
				memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact));
				cpSpacePushContacts(space, numContacts);
				
				// Reinsert the arbiter into the arbiter cache
				cpShape *a = arb->a, *b = arb->b;
				cpShape *shape_pair[] = {a, b};
				cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
				cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL);
				
				// Update the arbiter's state
				arb->stamp = space->stamp;
				arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type);
				cpArrayPush(space->arbiters, arb);
				
				cpfree(contacts);
			}
		}

// 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{cpCollisionType a, b;} ids = {a->collision_type, b->collision_type};
	cpHashValue collHashID = CP_HASH_PAIR(a->collision_type, b->collision_type);
	cpCollisionHandler *handler = (cpCollisionHandler *)cpHashSetFind(space->collFuncSet, collHashID, &ids);
	
	int 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 = NULL;
	int numContacts = cpCollideShapes(a, b, &contacts);
	if(!numContacts) return; // Shapes are not colliding.
	
	// 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(a, b);
	cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, arbHashID, shape_pair, NULL);
	cpArbiterUpdate(arb, contacts, numContacts, handler, a, b); // retains the contacts array
	
	// Call the begin function first if we need to
	int beginPass = (arb->stamp >= 0) || (handler->begin(arb, space, handler->data));
	if(beginPass && handler->preSolve(arb, space, handler->data) && !sensor){
		cpArrayPush(space->arbiters, arb);
	} else {
		cpfree(arb->contacts);
		arb->contacts = NULL;
	}
	
	// Time stamp the arbiter so we know it was used recently.
	arb->stamp = space->stamp;
}

void
cpSpaceActivateBody(cpSpace *space, cpBody *body)
{
	cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rouge body.");
	
	if(space->locked){
		// cpSpaceActivateBody() is called again once the space is unlocked
		if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body);
	} else {
		cpArrayPush(space->bodies, body);

		CP_BODY_FOREACH_SHAPE(body, shape){
			cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid);
			cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid);
		}
		
		CP_BODY_FOREACH_ARBITER(body, arb){
			cpBody *bodyA = arb->body_a;
			if(body == bodyA || cpBodyIsStatic(bodyA)){
				int numContacts = arb->numContacts;
				cpContact *contacts = arb->contacts;
				
				// Restore contact values back to the space's contact buffer memory
				arb->contacts = cpContactBufferGetArray(space);
				memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact));
				cpSpacePushContacts(space, numContacts);
				
				// Reinsert the arbiter into the arbiter cache
				cpShape *a = arb->a, *b = arb->b;
				cpShape *shape_pair[] = {a, b};
				cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b);
				cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL);
				
				// Update the arbiter's state
				arb->stamp = space->stamp;
				arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type);
				cpArrayPush(space->arbiters, arb);
				
				cpfree(contacts);
			}
		}

void
cpBBTreeOptimize(cpSpatialIndex *index)
{
	if(index->klass != &klass){
		cpAssertWarn(cpFalse, "Ignoring cpBBTreeOptimize() call to non-tree spatial index.");
		return;
	}
	
	cpBBTree *tree = (cpBBTree *)index;
	Node *root = tree->root;
	if(!root) return;
	
	int count = cpBBTreeCount(tree);
	Node **nodes = (Node **)cpcalloc(count, sizeof(Node *));
	Node **cursor = nodes;
	
	cpHashSetEach(tree->leaves, (cpHashSetIteratorFunc)fillNodeArray, &cursor);
	
	SubtreeRecycle(tree, root);
	tree->root = partitionNodes(tree, nodes, count);
	cpfree(nodes);
}

void
cpSpaceUnlock(cpSpace *space, cpBool runPostStep)
{
	space->locked--;
	cpAssertHard(space->locked >= 0, "Internal Error: Space lock underflow.");
	
	if(space->locked == 0){
		cpArray *waking = space->rousedBodies;
		
		for(int i=0, count=waking->num; i<count; i++){
			cpSpaceActivateBody(space, (cpBody *)waking->arr[i]);
			waking->arr[i] = NULL;
		}
		
		waking->num = 0;
		
		if(space->locked == 0 && runPostStep && !space->skipPostStep){
			space->skipPostStep = cpTrue;
			
			cpArray *arr = space->postStepCallbacks;
			for(int i=0; i<arr->num; i++){
				cpPostStepCallback *callback = (cpPostStepCallback *)arr->arr[i];
				cpPostStepFunc func = callback->func;
				
				// Mark the func as NULL in case calling it calls cpSpaceRunPostStepCallbacks() again.
				// TODO: need more tests around this case I think.
				callback->func = NULL;
				if(func) func(space, callback->key, callback->data);
				
				arr->arr[i] = NULL;
				cpfree(callback);
			}
			
			arr->num = 0;
			space->skipPostStep = cpFalse;
		}
	}
}

// The looping and sample caching code is shared between cpMarchHard() and cpMarchSoft().
static void
cpMarchCells(
  cpBB bb, unsigned long x_samples, unsigned long y_samples, cpFloat t,
  cpMarchSegmentFunc segment, void *segment_data,
  cpMarchSampleFunc sample, void *sample_data,
	cpMarchCellFunc cell
){
	cpFloat x_denom = 1.0/(cpFloat)(x_samples - 1);
	cpFloat y_denom = 1.0/(cpFloat)(y_samples - 1);

	// TODO range assertions and short circuit for 0 sized windows.

	// Keep a copy of the previous row to avoid double lookups.
	cpFloat *buffer = (cpFloat *)cpcalloc(x_samples, sizeof(cpFloat));
	for(unsigned long i=0; i<x_samples; i++) buffer[i] = sample(cpv(cpflerp(bb.l, bb.r, i*x_denom), bb.b), sample_data);

	for(unsigned long j=0; j<y_samples-1; j++){
		cpFloat y0 = cpflerp(bb.b, bb.t, (j+0)*y_denom);
		cpFloat y1 = cpflerp(bb.b, bb.t, (j+1)*y_denom);

		cpFloat a, b = buffer[0];
		cpFloat c, d = sample(cpv(bb.l, y1), sample_data);
		buffer[0] = d;

		for(unsigned long i=0; i<x_samples-1; i++){
			cpFloat x0 = cpflerp(bb.l, bb.r, (i+0)*x_denom);
			cpFloat x1 = cpflerp(bb.l, bb.r, (i+1)*x_denom);

			a = b, b = buffer[i + 1];
			c = d, d = sample(cpv(x1, y1), sample_data);
			buffer[i + 1] = d;

			cell(t, a, b, c, d, x0, x1, y0, y1, segment, segment_data);
		}
	}

	cpfree(buffer);
}

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;
				}
			}
		}

		cpfree(arb->contacts);
	}
	
	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;
}

void
cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data)
{
	// Iterate over all the chains.
	for(int i=0; i<set->size; i++){
		// The rest works similarly to cpHashSetRemove() above.
		cpHashSetBin **prev_ptr = &set->table[i];
		cpHashSetBin *bin = set->table[i];
		while(bin){
			cpHashSetBin *next = bin->next;
			
			if(func(bin->elt, data)){
				prev_ptr = &bin->next;
			} else {
				(*prev_ptr) = next;

				set->entries--;
				cpfree(bin);
			}
			
			bin = next;
		}
	}
}

// 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);
}