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

#include "dat.h"

#define TILE_SIZE 16

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

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

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

static BodyID bids[0x10000];
static int    bidsi;


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

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

		mapcollision(b);
		bids[bidsi++] = i;
		if(bidsi >= LENGTH(bids)) {
			__phxbroadphase(bidsi, bids);
			bidsi = 0;
		}
	}
	__phxbroadphase(bidsi, bids);

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