#include <math.h>
#include <stdbool.h>
#include <stddef.h>

#include "dat.h"

static void mink(Body *a, Body *b, float *minx, float *miny, float *maxx, float *maxy);
static int checkcollision(Body *a, Body *b);
static void resolvecoll(Body *a, Body *b, float p[2]);
static void mapcollision(Body *a);

static EntityCollision ecoll[2048];
static int             ecollstart, ecollend, ecollsi;

void
entphysics(float delta)
{
	for(int i = 0; i < ebufi; i++) {
		Entity *e = entitybuffer + i;
		if(!e->active)
			continue;

		mapcollision(&e->body);

		for(int j = i; j >= 0; j--) {
			Entity *f = entitybuffer + j;
			if(!f->active || f == e)
				continue;

			if(checkcollision(&e->body, &f->body)) {
				EntityCollision cl;

				cl.self = e;
				cl.target = f;

				enqcoll(&cl);
			}
		}
	}

	for(int i = 0; i < ebufi; i++) {
		Entity *e = entitybuffer + i;
		if(!e->active)
			continue;

		for(int j = 0; j < 2; j++)
			e->body.pos[j] += e->body.vel[j] * delta;
	}
}

int
checkcollision(Body *a, Body *b)
{
	float minx, miny, maxx, maxy;

	mink(a, b, &minx, &miny, &maxx, &maxy);

	return minx < 0 && maxx > 0 && miny < 0 && maxy > 0;
}

void
resolvecoll(Body *a, Body *b, float p[2])
{
	float minx, miny, maxx, maxy;
	mink(a, b, &minx, &miny, &maxx, &maxy);

	float hx = fabsf(minx) < fabsf(maxx) ? fabsf(minx) : fabsf(maxx);
	float hy = fabsf(miny) < fabsf(maxy) ? fabsf(miny) : fabsf(maxy);

	if(hx < hy) {
		p[1] = 0;
		p[0] = hx * (fabsf(minx) < fabsf(maxx) ? -1.0 : 1.0);
	} else {
		p[0] = 0;
		p[1] = hy * (fabsf(miny) < fabsf(maxy) ? -1.0 : 1.0);
	}
}

void 
mink(Body *a, Body *b, float *minx, float *miny, float *maxx, float *maxy)
{
	float p[2], s[2];
	
	for(int i = 0; i < 2; i++) {
		s[i] = a->size[i] + b->size[i];
		p[i] = a->pos[i] - b->pos[i];
	}

	*minx = p[0] - s[0];
	*maxx = p[0] + s[0];
	*miny = p[1] - s[1];
	*maxy = p[1] + s[1];
}

void
enqcoll(EntityCollision *c)
{
	if(ecollsi >= 2048)
		return;

	ecollsi++;

	ecoll[ecollend++] = *c;
	if(ecollend >= 2048)
		ecollend = 0;
}

EntityCollision *
deqcoll(void)
{
	EntityCollision *c;
	if(ecollsi <= 0)
		return NULL;

	ecollsi--;

	c = ecoll + ecollstart++;
	
	if(ecollstart >= 2048)
		ecollstart = 0;

	return c;
}

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

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

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

			if(tilex < 0 || tilex >= map_width)
				continue;
			if(tiley < 0 || tiley >= map_height)
				continue;

			if(!map_data[tilex + tiley * map_width])
				continue;

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

			if(checkcollision(a, &b)) {
				float p[2];
				resolvecoll(a, &b, p);

				for(int i = 0; i < 2; i++)
					a->pos[i] -= p[i];

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