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

#include "dat.h"

#define PHX_TICK_TIME (1.0 / 480.0)

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

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

#define TILE_SIZE 16

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
tick(void)
{
	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);
	}

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

		mapcollision(b);
		bodycollisionmap(b);
	}

	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
bodycollision(Body *a)
{
	CollisionEvent event;
	for(Body *b = a + 1; b < (a + phxbodypoolsize); b++) {
		if(!b->active)
			continue;

		if(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 = a - phxbodypool;
			event.body2 = b - phxbodypool;
			phxenqevent(&event);
		}
	}
}

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)
{
	CollisionEvent event;
	while(n) {
		Body *b = phxbodypool + n->id;
		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 = a - phxbodypool;
			event.body2 = b - phxbodypool;
			phxenqevent(&event);
		}

		n = n->next;
	}
}