#include <vecmath.h>
#include <physics.h>

#include <stdlib.h>
#include <string.h>

#include "dat.h"

#define BUCKETS      0x10000 /* must be a power of 2 */     

#define BUCKETMASK  (BUCKETS - 1)

#define PHI   1.618033988749895 
#define HMUL  (int)(BUCKETS / PHI)

static int hash(int a);
static void makeblockmap(Body *b);

static BlockmapNode *allocnode(void);

static BlockmapNode nodes[(1 << 20)];
static int nodesi;
static BlockmapNode *lists[BUCKETS];

void
phxmakeblkmap()
{
	nodesi = 0;

	for(int i = 0; i < BUCKETS; i++)
		lists[i] = NULL;

	for(int i = 0; i < phxbodypoolsize; i++) {
		Body *b = phxbodypool + i;
		if(!b->active)
			continue;
		makeblockmap(b);
	}
}

void
makeblockmap(Body *b)
{
	BlockmapNode *n;
	if((n = allocnode()) == NULL)
		return;

	n->id = b - phxbodypool;
	n->next = NULL;

	int mapx = (int)floorf(b->pos[0] / BLOCK_SIZE);
	int mapy = (int)floorf(b->pos[1] / BLOCK_SIZE);
	int h = hash(mapx + hash(mapy));

	n->next = lists[h];
	lists[h] = n;
}

int
hash(int a)
{
	return (a * HMUL) & BUCKETMASK;
}

BlockmapNode *
allocnode(void)
{
	if(nodesi == (sizeof(nodes) / sizeof(nodes[0])))
		return NULL;
	return nodes + nodesi++;
}

BlockmapNode *
phxnodelist(float x, float y)
{
	int mapx = (int)floorf(x / BLOCK_SIZE);
	int mapy = (int)floorf(y / BLOCK_SIZE);
	int h = hash(mapx + hash(mapy));
	return lists[h];
}