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

		for(int y = 0; y < map_height; y++)
			for(int x = 0; x < map_width; x++) {
				if(!map_data[x + y * map_width])
					continue;

				Body b = {
					.pos = {
						x * ENTITY_SIZE * 2.0,
						y * ENTITY_SIZE * 2.0,
					},
					.size = { ENTITY_SIZE, ENTITY_SIZE },
				};

				if(checkcollision(&e->body, &b)) {
					float p[2];
					resolvecoll(&e->body, &b, p);

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

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

		for(int j = i; j < ebufi; j++) {
			Entity *f = entitybuffer + j;
			if(!f->active)
				break;

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