// written in the D programming language

module chipmunkd.cpSpaceHash;

import chipmunkd.chipmunk;
import chipmunkd.chipmunk_types_h;
import chipmunkd.cpVect,chipmunkd.cpVect_h;
import chipmunkd.cpBB;
import chipmunkd.cpHashSet;
import chipmunkd.cpArray;
import chipmunkd.prime;

// The spatial hash is Chipmunk's default (and currently only) spatial index type.
// Based on a chained hash table.

// Used internally to track objects added to the hash
struct cpHandle{
	// Pointer to the object
	void *obj;
	// Retain count
	int retain;
	// Query stamp. Used to make sure two objects
	// aren't identified twice in the same query.
	cpTimestamp stamp;
}

// Linked list element for in the chains.
struct cpSpaceHashBin{
	cpHandle *handle;
	cpSpaceHashBin *next;
}

// BBox callback. Called whenever the hash needs a bounding box from an object.
alias cpBB function(void *obj)cpSpaceHashBBFunc;

struct cpSpaceHash{
	// Number of cells in the table.
	int numcells;
	// Dimentions of the cells.
	cpFloat celldim;
	
	// BBox callback.
	cpSpaceHashBBFunc bbfunc;

	// Hashset of the handles and the recycled ones.
	cpHashSet *handleSet;
	cpArray *pooledHandles;
	
	// The table and the recycled bins.
	cpSpaceHashBin **table;
	cpSpaceHashBin *pooledBins;
	
	// list of buffers to free on destruction.
	cpArray *allocatedBuffers;
	
	// Incremented on each query. See cpHandle.stamp.
	cpTimestamp stamp;
}

////Basic allocation/destruction functions.
//cpSpaceHash *cpSpaceHashAlloc(void);
//cpSpaceHash *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc);
//cpSpaceHash *cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc);
//
//void cpSpaceHashDestroy(cpSpaceHash *hash);
//void cpSpaceHashFree(cpSpaceHash *hash);
//
//// Resize the hashtable. (Does not rehash! You must call cpSpaceHashRehash() if needed.)
//void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells);
//
//// Add an object to the hash.
//void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB _deprecated_ignored);
//// Remove an object from the hash.
//void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue id);
//
//// Iterator function
alias void function(void *obj, void *data)cpSpaceHashIterator;
//// Iterate over the objects in the hash.
//void cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data);
//
//// Rehash the contents of the hash.
//void cpSpaceHashRehash(cpSpaceHash *hash);
//// Rehash only a specific object.
//void cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue id);
//
//// Query callback.
alias void function(void *obj1, void *obj2, void *data)cpSpaceHashQueryFunc;
//// Point query the hash. A reference to the query point is passed as obj1 to the query callback.
//void cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data);
//// Query the hash for a given BBox.
//void cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data);
//// Run a query for the object, then insert it. (Optimized case)
//void cpSpaceHashQueryInsert(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data);
//// Rehashes while querying for each object. (Optimized case) 
//void cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data);
//
//// Segment Query callback.
//// Return value is uesd for early exits of the query.
//// If while traversing the grid, the raytrace function detects that an entire grid cell is beyond the hit point, it will stop the trace.
alias cpFloat function(void *obj1, void *obj2, void *data)cpSpaceHashSegmentQueryFunc;
//void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpaceHashSegmentQueryFunc func, void *data);
//

static cpHandle*
cpHandleInit(cpHandle *hand, void *obj)
{
	hand.obj = obj;
	hand.retain = 0;
	hand.stamp = 0;
	
	return hand;
}

static void cpHandleRetain(cpHandle *hand){hand.retain++;}

static void
cpHandleRelease(cpHandle *hand, cpArray *pooledHandles)
{
	hand.retain--;
	if(hand.retain == 0) cpArrayPush(pooledHandles, hand);
}

cpSpaceHash*
cpSpaceHashAlloc()
{
	return cast(cpSpaceHash *)cpcalloc(1, cpSpaceHash.sizeof);
}

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

// Equality function for the handleset.
static int handleSetEql(void *obj, cpHandle* hand){return (obj == hand.obj);}

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

cpSpaceHash*
cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBFunc bbfunc)
{
	cpSpaceHashAllocTable(hash, next_prime(numcells));
	hash.celldim = celldim;
	hash.bbfunc = bbfunc;
	
	hash.handleSet = cpHashSetNew(0, cast(cpHashSetEqlFunc)&handleSetEql, cast(cpHashSetTransFunc)&handleSetTrans);
	hash.pooledHandles = cpArrayNew(0);
	
	hash.pooledBins = null;
	hash.allocatedBuffers = cpArrayNew(0);
	
	hash.stamp = 1;
	
	return hash;
}

cpSpaceHash*
cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc)
{
	return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc);
}

static void
recycleBin(cpSpaceHash *hash, cpSpaceHashBin *bin)
{
	bin.next = hash.pooledBins;
	hash.pooledBins = bin;
}

static void
clearHashCell(cpSpaceHash *hash, int idx)
{
	cpSpaceHashBin *bin = hash.table[idx];
	while(bin){
		cpSpaceHashBin *next = bin.next;
		
		cpHandleRelease(bin.handle, hash.pooledHandles);
		recycleBin(hash, bin);
		
		bin = next;
	}
	
	hash.table[idx] = null;
}

// Clear all cells in the hashtable.
static void
clearHash(cpSpaceHash *hash)
{
	for(int i=0; i<hash.numcells; i++)
		clearHashCell(hash, i);
}

static void freeWrap(void *ptr, void *unused){cpfree(ptr);}

void
cpSpaceHashDestroy(cpSpaceHash *hash)
{
	clearHash(hash);
	
	cpHashSetFree(hash.handleSet);
	
	cpArrayEach(hash.allocatedBuffers, &freeWrap, null);
	cpArrayFree(hash.allocatedBuffers);
	cpArrayFree(hash.pooledHandles);
	
	cpfree(hash.table);
}

void
cpSpaceHashFree(cpSpaceHash *hash)
{
	if(hash){
		cpSpaceHashDestroy(hash);
		cpfree(hash);
	}
}

void
cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells)
{
	// Clear the hash to release the old handle locks.
	clearHash(hash);
	
	hash.celldim = celldim;
	cpSpaceHashAllocTable(hash, next_prime(numcells));
}

// Return true if the chain contains the handle.
static cpBool
containsHandle(cpSpaceHashBin *bin, cpHandle *hand)
{
	while(bin){
		if(bin.handle == hand) return cpTrue;
		bin = bin.next;
	}
	
	return cpFalse;
}

// Get a recycled or new bin.
static 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/cpSpaceHashBin.sizeof;
		assert(count, "Buffer size is too small.");
		
		cpSpaceHashBin *buffer = cast(cpSpaceHashBin *)cpmalloc(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;
	}
}

// The hash function itself.
static cpHashValue
hash_func(cpHashValue x, cpHashValue y, cpHashValue n)
{
	return cast(cpHashValue)((x*1640531513uL ^ y*2654435789uL) % n);
}

// Much faster than (int)floor(f)
// Profiling showed floor() to be a sizable performance hog
static int
floor_int(cpFloat f)
{
	int i = cast(int)f;
	return (f < 0.0f && f != i ? i - 1 : i);
}

static void
hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb)
{
	// Find the dimensions in cell coordinates.
	cpFloat dim = hash.celldim;
	int l = floor_int(bb.l/dim); // Fix by ShiftZ
	int r = floor_int(bb.r/dim);
	int b = floor_int(bb.b/dim);
	int t = floor_int(bb.t/dim);
	
	int n = hash.numcells;
	for(int i=l; i<=r; i++){
		for(int j=b; j<=t; j++){
			int idx = hash_func(i,j,n);
			cpSpaceHashBin *bin = hash.table[idx];
			
			// Don't add an object twice to the same cell.
			if(containsHandle(bin, hand)) continue;

			cpHandleRetain(hand);
			// Insert a new bin for the handle in this cell.
			cpSpaceHashBin *newBin = getEmptyBin(hash);
			newBin.handle = hand;
			newBin.next = bin;
			hash.table[idx] = newBin;
		}
	}
}

void
cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue hashid, cpBB _deprecated_unused)
{
	cpHandle *hand = cast(cpHandle *)cpHashSetInsert(hash.handleSet, hashid, obj, hash);
	hashHandle(hash, hand, hash.bbfunc(obj));
}

void
cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue hashid)
{
	cpHandle *hand = cast(cpHandle *)cpHashSetRemove(hash.handleSet, hashid, obj);
	
	if(hand){
		hand.obj = null;
		cpHandleRelease(hand, hash.pooledHandles);
		
		cpSpaceHashInsert(hash, obj, hashid, cpBBNew(0.0f, 0.0f, 0.0f, 0.0f));
	}
}

static void handleRehashHelper(cpHandle *hand, cpSpaceHash *hash){hashHandle(hash, hand, hash.bbfunc(hand.obj));}

void
cpSpaceHashRehash(cpSpaceHash *hash)
{
	clearHash(hash);
	cpHashSetEach(hash.handleSet, cast(cpHashSetIterFunc)&handleRehashHelper, hash);
}

void
cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue hashid)
{
	cpHandle *hand = cast(cpHandle *)cpHashSetRemove(hash.handleSet, hashid, obj);
	
	if(hand){
		hand.obj = null;
		cpHandleRelease(hand, hash.pooledHandles);
	}
}

struct eachPair {
	cpSpaceHashIterator func;
	void *data;
}

static void eachHelper(cpHandle *hand, eachPair *pair){pair.func(hand.obj, pair.data);}

// Iterate over the objects in the spatial hash.
void
cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data)
{
	eachPair pair = {func, data};
	cpHashSetEach(hash.handleSet, cast(cpHashSetIterFunc)&eachHelper, &pair);
}

static void
removeOrphanedHandles(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr)
{
	cpSpaceHashBin *bin = *bin_ptr;
	while(bin){
		cpHandle *hand = bin.handle;
		cpSpaceHashBin *next = bin.next;
		
		if(!hand.obj){
			// orphaned handle, unlink and recycle the bin
			(*bin_ptr) = bin.next;
			recycleBin(hash, bin);
			
			cpHandleRelease(hand, hash.pooledHandles);
		} else {
			bin_ptr = &bin.next;
		}
		
		bin = next;
	}
}

// Calls the callback function for the objects in a given chain.
static void
query(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashQueryFunc func, void *data)
{
	restart:
	for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin.next){
		cpHandle *hand = bin.handle;
		void *other = hand.obj;
		
		if(hand.stamp == hash.stamp || obj == other){
			continue;
		} else if(other){
			func(obj, other, data);
			hand.stamp = hash.stamp;
		} else {
			// The object for this handle has been removed
			// cleanup this cell and restart the query
			removeOrphanedHandles(hash, bin_ptr);
			goto restart; // GCC not smart enough/able to tail call an inlined function.
		}
	}
}

void
cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data)
{
	cpFloat dim = hash.celldim;
	int idx = hash_func(floor_int(point.x/dim), floor_int(point.y/dim), hash.numcells);  // Fix by ShiftZ
	
	query(hash, &hash.table[idx], &point, func, data);
	hash.stamp++;
}

void
cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data)
{
	// Get the dimensions in cell coordinates.
	cpFloat dim = hash.celldim;
	int l = floor_int(bb.l/dim);  // Fix by ShiftZ
	int r = floor_int(bb.r/dim);
	int b = floor_int(bb.b/dim);
	int t = floor_int(bb.t/dim);
	
	int n = hash.numcells;
	cpSpaceHashBin **table = hash.table;
	
	// Iterate over the cells and query them.
	for(int i=l; i<=r; i++){
		for(int j=b; j<=t; j++){
			query(hash, &table[hash_func(i,j,n)], obj, func, data);
		}
	}
	
	hash.stamp++;
}

// Similar to struct eachPair above.
struct queryRehashPair {
	cpSpaceHash *hash;
	cpSpaceHashQueryFunc func;
	void *data;
}

// Hashset iterator func used with cpSpaceHashQueryRehash().
static void
handleQueryRehashHelper(void *elt, void *data)
{
	cpHandle *hand = cast(cpHandle *)elt;
	
	// Unpack the user callback data.
	queryRehashPair *pair = cast(queryRehashPair *)data;
	cpSpaceHash *hash = pair.hash;
	cpSpaceHashQueryFunc func = pair.func;

	cpFloat dim = hash.celldim;
	int n = hash.numcells;

	void *obj = hand.obj;
	cpBB bb = hash.bbfunc(obj);

	int l = floor_int(bb.l/dim);
	int r = floor_int(bb.r/dim);
	int b = floor_int(bb.b/dim);
	int t = floor_int(bb.t/dim);
	
	cpSpaceHashBin **table = hash.table;

	for(int i=l; i<=r; i++){
		for(int j=b; j<=t; j++){
			int idx = hash_func(i,j,n);
			cpSpaceHashBin *bin = table[idx];
			
			if(containsHandle(bin, hand)) continue;
			
			cpHandleRetain(hand); // this MUST be done first in case the object is removed in func()
			query(hash, &bin, obj, func, pair.data);
			
			cpSpaceHashBin *newBin = getEmptyBin(hash);
			newBin.handle = hand;
			newBin.next = bin;
			table[idx] = newBin;
		}
	}
	
	// Increment the stamp for each object hashed.
	hash.stamp++;
}

void
cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data)
{
	clearHash(hash);
	
	queryRehashPair pair = {hash, func, data};
	cpHashSetEach(hash.handleSet, &handleQueryRehashHelper, &pair);
}

static cpFloat
segmentQuery(cpSpaceHash *hash, cpSpaceHashBin **bin_ptr, void *obj, cpSpaceHashSegmentQueryFunc func, void *data)
{
	cpFloat t = 1.0f;
	 
	restart:
	for(cpSpaceHashBin *bin = *bin_ptr; bin; bin = bin.next){
		cpHandle *hand = bin.handle;
		void *other = hand.obj;
		
		// Skip over certain conditions
		if(hand.stamp == hash.stamp){
			continue;
		} else if(other){
			t = cpfmin(t, func(obj, other, data));
			hand.stamp = hash.stamp;
		} else {
			// The object for this handle has been removed
			// cleanup this cell and restart the query
			removeOrphanedHandles(hash, bin_ptr);
			goto restart; // GCC not smart enough/able to tail call an inlined function.
		}
	}
	
	return t;
}

// modified from http://playtechs.blogspot.com/2007/03/raytracing-on-grid.html
void cpSpaceHashSegmentQuery(cpSpaceHash *hash, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpaceHashSegmentQueryFunc func, void *data)
{
	a = cpvmult(a, 1.0f/hash.celldim);
	b = cpvmult(b, 1.0f/hash.celldim);
	
	int cell_x = floor_int(a.x), cell_y = floor_int(a.y);

	cpFloat t = 0;

	int x_inc, y_inc;
	cpFloat temp_v, temp_h;

	if (b.x > a.x){
		x_inc = 1;
		temp_h = (cpffloor(a.x + 1.0f) - a.x);
	} else {
		x_inc = -1;
		temp_h = (a.x - cpffloor(a.x));
	}

	if (b.y > a.y){
		y_inc = 1;
		temp_v = (cpffloor(a.y + 1.0f) - a.y);
	} else {
		y_inc = -1;
		temp_v = (a.y - cpffloor(a.y));
	}
	
	// Division by zero is *very* slow on ARM
	cpFloat dx = cpfabs(b.x - a.x), dy = cpfabs(b.y - a.y);
	cpFloat dt_dx = (dx ? 1.0f/dx : INFINITY), dt_dy = (dy ? 1.0f/dy : INFINITY);
	
	// fix NANs in horizontal directions
	cpFloat next_h = (temp_h ? temp_h*dt_dx : dt_dx);
	cpFloat next_v = (temp_v ? temp_v*dt_dy : dt_dy);
	
	cpSpaceHashBin **table = hash.table;

	int n = hash.numcells;
	while(t < t_exit){
		int idx = hash_func(cell_x, cell_y, n);
		t_exit = cpfmin(t_exit, segmentQuery(hash, &table[idx], obj, func, data));

		if (next_v < next_h){
			cell_y += y_inc;
			t = next_v;
			next_v += dt_dy;
		} else {
			cell_x += x_inc;
			t = next_h;
			next_h += dt_dx;
		}
	}
	
	hash.stamp++;
}