#include "collisions.h"

#include "entity.h"

#include <assert.h>
#include <string.h>

typedef struct {
	bool valid;
	unsigned component_id;
	float r;
	unsigned priority;
	collision_cb_t cb;
} entry_t;

static unsigned max;
static entry_t entries[MAX_ENTITIES];

static void update(unsigned new_entity_id, void *ref)
{
	entry_t *old_e = (entry_t *)ref;
	entry_t *new_e = entries + new_entity_id;
	memcpy(new_e, old_e, sizeof(entry_t));
	old_e->valid = false;
	entity_update_component(new_entity_id, new_e->component_id, new_e);
}

static void remove(void *ref)
{
	entry_t *e = (entry_t *)ref;
	e->valid = false;
}

void collisions_init()
{
	max = 0;
}

void collisions_update()
{
	for (unsigned i = 0; i < max; ++i) {
		if (!entries[i].valid)
			continue;

		for (unsigned j = i + 1; j < max; ++j) {
			if (!entries[j].valid)
				continue;

			const physics_sep_t sep = physics_separation(i, j);
			if (sep.dist > entries[i].r + entries[j].r)
				continue;
			if (sep.va - sep.vb <= 0)
				continue;

			if (entries[i].priority >= entries[j].priority) {
				entries[i].cb(i, j, sep);
			} else {
				const physics_sep_t flip_sep = {
					.dist = sep.dist,
					.norm = vec2_scale(sep.norm, -1),
					.va = -sep.vb,
					.vb = -sep.va,
				};
				entries[j].cb(j, i, flip_sep);
			}
		}
	}
}

void collisions_add(
    unsigned entity, float radius, unsigned priority, collision_cb_t cb)
{
	assert(entity < MAX_ENTITIES);
	if (entity >= max)
		max = entity + 1;
	else
		assert(!entries[entity].valid);

	entry_t *entry = entries + entity;
	entry->valid = true;
	entry->component_id
	    = entity_add_component(entity, update, remove, entry);

	entry->r = radius;
	entry->priority = priority;
	entry->cb = cb;
}