// Transformation function for collisionHandlers.
static void *
handlerSetTrans(cpCollisionHandler *handler, void *unused)
{
	cpCollisionHandler *copy = (cpCollisionHandler *)cpcalloc(1, sizeof(cpCollisionHandler));
	memcpy(copy, handler, sizeof(cpCollisionHandler));
	
	return copy;
}

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

static void *
postStepFuncSetTrans(cpPostStepCallback *callback, void *ignored)
{
	cpPostStepCallback *value = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback));
	(*value) = (*callback);
	
	return value;
}

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

cpHashSet *
cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc)
{
	cpHashSet *set = (cpHashSet *)cpcalloc(1, sizeof(cpHashSet));
	
	set->size = next_prime(size);
	set->entries = 0;
	
	set->eql = eqlFunc;
	set->default_value = NULL;
	
	set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *));
	set->pooledBins = NULL;
	
	set->allocatedBuffers = cpArrayNew(0);
	
	return set;
}

cpPolylineSet *
cpPolylineSetInit(cpPolylineSet *set)
{
	set->count = 0;
	set->capacity = 8;
	set->lines = cpcalloc(set->capacity, sizeof(cpPolyline));
	
  return set;
}

static cpPolyline *
cpPolylineMake(int capacity)
{
	capacity = (capacity > DEFAULT_POLYLINE_CAPACITY ? capacity : DEFAULT_POLYLINE_CAPACITY);
	
	cpPolyline *line = (cpPolyline *)cpcalloc(1, cpPolylineSizeForCapacity(capacity));
	line->count = 0;
	line->capacity = capacity;
	
	return line;
}

static void
setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset)
{
	poly->numVerts = numVerts;

	poly->verts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
	poly->tVerts = (cpVect *)cpcalloc(numVerts, sizeof(cpVect));
	poly->axes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
	poly->tAxes = (cpPolyShapeAxis *)cpcalloc(numVerts, sizeof(cpPolyShapeAxis));
	
	for(int i=0; i<numVerts; i++){
		cpVect a = cpvadd(offset, verts[i]);
		cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));

		poly->verts[i] = a;
		poly->axes[i].n = n;
		poly->axes[i].d = cpvdot(n, a);
	}
}

	// Initializes the array of collision functions.
	// Called by cpInitChipmunk().
	void
	cpInitCollisionFuncs(void)
	{
		if(!colfuncs)
			colfuncs = (collisionFunc *)cpcalloc(CP_NUM_SHAPES*CP_NUM_SHAPES, sizeof(collisionFunc));
		
		addColFunc(CP_CIRCLE_SHAPE,  CP_CIRCLE_SHAPE,  circle2circle);
		addColFunc(CP_CIRCLE_SHAPE,  CP_SEGMENT_SHAPE, circle2segment);
		addColFunc(CP_SEGMENT_SHAPE, CP_POLY_SHAPE,    seg2poly);
		addColFunc(CP_CIRCLE_SHAPE,  CP_POLY_SHAPE,    circle2poly);
		addColFunc(CP_POLY_SHAPE,    CP_POLY_SHAPE,    poly2poly);
	}	

static void
setUpVerts(cpPolyShape *poly, int numVerts, const cpVect *verts, cpVect offset)
{
    // Fail if the user attempts to pass a concave poly, or a bad winding.
    cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL().");

    poly->numVerts = numVerts;
    poly->verts = (cpVect *)cpcalloc(2*numVerts, sizeof(cpVect));
    poly->planes = (cpSplittingPlane *)cpcalloc(2*numVerts, sizeof(cpSplittingPlane));
    poly->tVerts = poly->verts + numVerts;
    poly->tPlanes = poly->planes + numVerts;

    for(int i=0; i<numVerts; i++) {
        cpVect a = cpvadd(offset, verts[i]);
        cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
        cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));

        poly->verts[i] = a;
        poly->planes[i].n = n;
        poly->planes[i].d = cpvdot(n, a);
    }

}

cpHashSet *
cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans)
{
	set->size = next_prime(size);
	set->entries = 0;
	
	set->eql = eqlFunc;
	set->trans = trans;
	
	set->default_value = NULL;
	
	set->table = (cpHashSetBin **)cpcalloc(set->size, sizeof(cpHashSetBin *));
	
	return set;
}

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

cpBool
cpSpaceAddPostStepCallback(cpSpace *space, cpPostStepFunc func, void *key, void *data)
{
	cpAssertWarn(space->locked,
		"Adding a post-step callback when the space is not locked is unnecessary. "
		"Post-step callbacks will not called until the end of the next call to cpSpaceStep() or the next query.");
	
	if(!cpSpaceGetPostStepCallback(space, key)){
		cpPostStepCallback *callback = (cpPostStepCallback *)cpcalloc(1, sizeof(cpPostStepCallback));
		callback->func = (func ? func : PostStepDoNothing);
		callback->key = key;
		callback->data = data;
		
		cpArrayPush(space->postStepCallbacks, callback);
		return cpTrue;
	} else {
		return cpFalse;
	}
}

static void
SetVerts(cpPolyShape *poly, int count, const cpVect *verts)
{
	poly->count = count;
	if(count <= CP_POLY_SHAPE_INLINE_ALLOC){
		poly->planes = poly->_planes;
	} else {
		poly->planes = (struct cpSplittingPlane *)cpcalloc(2*count, sizeof(struct cpSplittingPlane));
	}
	
	for(int i=0; i<count; i++){
		cpVect a = verts[(i - 1 + count)%count];
		cpVect b = verts[i];
		cpVect n = cpvnormalize(cpvrperp(cpvsub(b, a)));
		
		poly->planes[i + count].v0 = b;
		poly->planes[i + count].n = n;
	}
}

static void *
handleSetTrans(void *obj, cpSpaceHash *hash)
{
	if(hash->pooledHandles->num == 0){
		// handle pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(cpHandle);
		cpAssertSoft(count, "Buffer size is too small.");
		
		cpHandle *buffer = (cpHandle *)cpcalloc(1, CP_BUFFER_BYTES);
		cpArrayPush(hash->allocatedBuffers, buffer);
		
		for(int i=0; i<count; i++) cpArrayPush(hash->pooledHandles, buffer + i);
	}
	
	cpHandle *hand = cpHandleInit((cpHandle *)cpArrayPop(hash->pooledHandles), obj);
	cpHandleRetain(hand);
	
	return hand;
}

cpSpace *
cpHastySpaceNew(void)
{
    cpHastySpace *hasty = (cpHastySpace *)cpcalloc(1, sizeof(cpHastySpace));
    cpSpaceInit((cpSpace *)hasty);

    pthread_mutex_init(&hasty->mutex, NULL);
    pthread_cond_init(&hasty->cond_work, NULL);
    pthread_cond_init(&hasty->cond_resume, NULL);

    // TODO magic number, should test this more thoroughly.
    hasty->constraint_count_threshold = 50;

    // Default to 1 thread for determinism.
    hasty->num_threads = 1;
    cpHastySpaceSetThreads((cpSpace *)hasty, 1);

    return (cpSpace *)hasty;
}

static Node *
NodeFromPool(cpBBTree *tree)
{
	Node *node = tree->pooledNodes;
	
	if(node){
		tree->pooledNodes = node->parent;
		return node;
	} else {
		// Pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(Node);
		cpAssertHard(count, "Internal Error: Buffer size is too small.");
		
		Node *buffer = (Node *)cpcalloc(1, CP_BUFFER_BYTES);
		cpArrayPush(tree->allocatedBuffers, buffer);
		
		// push all but the first one, return the first instead
		for(int i=1; i<count; i++) NodeRecycle(tree, buffer + i);
		return buffer;
	}
}

static cpHashSetBin *
getUnusedBin(cpHashSet *set)
{
	cpHashSetBin *bin = set->pooledBins;
	
	if(bin){
		set->pooledBins = bin->next;
		return bin;
	} else {
		// Pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin);
		cpAssertHard(count, "Internal Error: Buffer size is too small.");
		
		cpHashSetBin *buffer = (cpHashSetBin *)cpcalloc(1, CP_BUFFER_BYTES);
		cpArrayPush(set->allocatedBuffers, buffer);
		
		// push all but the first one, return it instead
		for(int i=1; i<count; i++) recycleBin(set, buffer + i);
		return buffer;
	}
}

// Get a recycled or new bin.
static inline cpSpaceHashBin *
getEmptyBin(cpSpaceHash *hash)
{
	cpSpaceHashBin *bin = hash->pooledBins;
	
	if(bin){
		hash->pooledBins = bin->next;
		return bin;
	} else {
		// Pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin);
		cpAssertSoft(count, "Buffer size is too small.");
		
		cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpcalloc(1, CP_BUFFER_BYTES);
		cpArrayPush(hash->allocatedBuffers, buffer);
		
		// push all but the first one, return the first instead
		for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
		return buffer;
	}
}

static Pair *
PairFromPool(cpBBTree *tree)
{
    Pair *pair = tree->pooledPairs;
    
    if(pair){
        tree->pooledPairs = pair->a.next;
        return pair;
    } else {
        // Pool is exhausted, make more
        int count = CP_BUFFER_BYTES/sizeof(Pair);
        cpAssertHard(count, "Internal Error: Buffer size is too small.");
        
        Pair *buffer = (Pair *)cpcalloc(1, CP_BUFFER_BYTES);
        cpArrayPush(tree->allocatedBuffers, buffer);
        
        // push all but the first one, return the first instead
        for(int i=1; i<count; i++) PairRecycle(tree, buffer + i);
        return buffer;
    }
}