#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)

#define LENGTH(X) (sizeof (X) / sizeof (X)[0])

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

static void enqueuetest(TestCollision *t);
static void bodycollisionmap(Body *);
static void bodycollisionmapsub(Body *, BlockmapNode *);
static BlockmapNode *allocnode(void);

static void phxmakeblkmap(void);
static BlockmapNode *phxnodelist(float x, float y);

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

static TestCollision testcollision[0x1000];
static int testcollisioncount;

void
__phxbroadphasereset(void)
{
	static int count = 0;
	if(count == 0)
		phxmakeblkmap();
	count = (count + 1) % 4;
}

void 
__phxbroadphase(int n, BodyID bid[n])
{
	testcollisioncount = 0;
	for(int i = 0; i < n; i++) {
		Body *b = phxbodypool + bid[i];
		bodycollisionmap(b);
	}
	__phxnarrowphase(testcollisioncount, testcollision);
}

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

void
bodycollisionmap(Body *a)
{
	for(int y = -1; y <= 1; y++)
	for(int x = -1; x <= 1; x++) {
		bodycollisionmapsub(a, phxnodelist(
			a->pos[0] + x * BLOCK_SIZE, 
			a->pos[1] + y * BLOCK_SIZE
		));	
	}
}

void
bodycollisionmapsub(Body *a, BlockmapNode *n)
{
	while(n) {
		Body *b = phxbodypool + n->id;
		enqueuetest(&(TestCollision) {
			.a = a - phxbodypool,
			.b = b - phxbodypool
		});
		n = n->next;
	}
}

void
enqueuetest(TestCollision *t)
{
	testcollision[testcollisioncount++] = *t;
	if(testcollisioncount >= LENGTH(testcollision)) {
		__phxnarrowphase(testcollisioncount, testcollision);
		testcollisioncount = 0;
	}
}