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

#include "dat.h"

#define TILE_SIZE 16
#define TEST_BUFFER_SIZE 0x10000

typedef struct TestCollision TestCollision;
struct TestCollision {
	BodyID a, b;
};

static void tick(void);
static void mapcollision(Body *);
static void bodycollision(Body *);

static void bodycollisionmap(Body *);
static void bodycollisionmapsub(Body *, BlockmapNode *);

static void enqueuetest(TestCollision *t);
static void resolve(void);

static TestCollision testcollision[TEST_BUFFER_SIZE];
static int testcollisioncount;

void
phxtick(float delta)
{
	static float time;

	time += delta;
	if(time >= PHX_TICK_TIME) {
		while(time >= PHX_TICK_TIME) {
			tick();
			time -= PHX_TICK_TIME;
		}

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

			b->accel[0] = 0.0;
			b->accel[1] = 0.0;
		}
	}
}

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

		b->accel[0] = 0.0;
		b->accel[1] = 0.0;
	}
}

void
tick(void)
{
	static int count = 0;

	if(!count) {
		phxmakeblkmap();
	}
	count = (count + 1) % 4;

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

		vec2_add_scaled(b->vel, b->vel, b->accel, PHX_TICK_TIME);
	}

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

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

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

		vec2_add_scaled(b->pos, b->pos, b->vel, PHX_TICK_TIME);
	}
}

void
mapcollision(Body *a)
{
	int minx, miny, maxx, maxy;

	minx = ((int)(a->pos[0] - a->size[0]) / TILE_SIZE) * TILE_SIZE;
	maxx = ((int)(a->pos[0] + a->size[0]) / TILE_SIZE) * TILE_SIZE;
	miny = ((int)(a->pos[1] - a->size[1]) / TILE_SIZE) * TILE_SIZE;
	maxy = ((int)(a->pos[1] + a->size[1]) / TILE_SIZE) * TILE_SIZE;

	for(int y = miny; y <= maxy + TILE_SIZE; y += TILE_SIZE)
		for(int x = minx; x <= maxx + TILE_SIZE; x += TILE_SIZE) {
			int tilex = x / (2 * TILE_SIZE);
			int tiley = y / (2 * TILE_SIZE);

			if(tilex < 0 || tilex >= phxmapwidth)
				continue;
			if(tiley < 0 || tiley >= phxmapheight)
				continue;

			if(!phxmapbuffer[tilex + tiley * phxmapwidth])
				continue;

			Body b = {
				.pos = {
					tilex * TILE_SIZE * 2.0,
					tiley * TILE_SIZE * 2.0,
				},
				.size = { TILE_SIZE, TILE_SIZE },
			};

			if(phxaabbcheck(a, &b)) {
				float p[2], n[2];
				phxaabbresolv(a, &b, p, n);

				vec2_sub(a->pos, a->pos, p);

				if(p[0] != 0.0) {
					a->vel[0] = 0;
				} else {
					a->vel[1] = 0;
				}
			}
		}
}

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 >= TEST_BUFFER_SIZE)
		resolve();
}

void
resolve(void)
{
	CollisionEvent event;

	for(int i = 0; i < testcollisioncount; i++) {
		TestCollision *c = testcollision + i;
		Body *a = c->a + phxbodypool;
		Body *b = c->b + phxbodypool;

		if(a < b && phxaabbcheck(a, b)) {
			float p[2], n[2], rvel[2], j;
			phxaabbresolv(a, b, p, n);

			vec2_sub(rvel, a->vel, b->vel);
			j = vec2_dot(rvel, n);

			vec2_sub_scaled(a->vel, a->vel, n,  0.5);
			vec2_sub_scaled(b->vel, b->vel, n, -0.5);

			vec2_sub_scaled(a->pos, a->pos, p,  0.5);
			vec2_sub_scaled(b->pos, b->pos, p, -0.5);

			event.body1 = c->a;
			event.body2 = c->b;
			phxenqevent(&event);
		}
	}
	testcollisioncount = 0;
}