#include "physics.h"

#include "entity.h"
#include "renderer.h"

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

typedef struct {
	bool valid;
	unsigned component_id;
	physics_t state;
} entry_t;

static unsigned max;
static unsigned escape_entity;
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 physics_init()
{
	max = 0;
	escape_entity = MAX_ENTITIES;
}

void physics_update()
{
	for (unsigned i = 0; i < max; ++i) {
		if (!entries[i].valid)
			continue;
		physics_t *st = &entries[i].state;

		st->dir = mat2_mul_mat2(mat2_rotation(st->rot), st->dir);
		st->pos = vec2_add(st->pos, st->vel);

		if (st->pos.y > 1)
			st->pos.y -= 2;
		else if (st->pos.y <= -1)
			st->pos.y += 2;

		if (st->pos.x >= aspect && i != escape_entity)
			st->pos.x -= 2 * aspect;
		else if (st->pos.x < -aspect)
			st->pos.x += 2 * aspect;
	}
}

void physics_add(
    unsigned entity, vec2_t pos, mat2_t dir, vec2_t vel, float rot,
    float mass)
{
	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->state = (physics_t) {
		.pos = pos,
		.dir = dir,
		.vel = vel,
		.rot = rot,
		.mass = mass,
	};
}

physics_t *physics_get(unsigned entity)
{
	assert(entity < max);
	assert(entries[entity].valid);
	return &entries[entity].state;
}

void physics_escape(unsigned entity)
{
	assert(entity < max);
	assert(entries[entity].valid);
	escape_entity = entity;
}

static void pos_views(vec2_t a, vec2_t b, vec2_t out[4])
{
	const float dx = 2 * aspect;
	const float dy = 2;
	out[0] = b;
	out[1] = (vec2_t) { b.x + copysign(dx, a.x), b.y };
	out[2] = (vec2_t) { b.x, b.y + copysign(dy, a.y) };
	out[3] = (vec2_t) { b.x + copysign(dx, a.x), b.y + copysign(dy, a.y) };
}

physics_sep_t physics_separation(unsigned a, unsigned b)
{
	const physics_t *pa = physics_get(a);
	const physics_t *pb = physics_get(b);

	vec2_t pos[4];
	pos_views(pa->pos, pb->pos, pos);

	float min_dist = INFINITY;
	vec2_t min_disp;
	for (unsigned i = 0; i < 4; ++i) {
		const vec2_t disp = vec2_sub(pos[i], pa->pos);
		const float dist = vec2_len(disp);
		if (dist < min_dist) {
			min_disp = disp;
			min_dist = dist;
		}
	}

	const vec2_t norm = vec2_norm(min_disp);
	return (physics_sep_t) {
		.dist = min_dist,
		.norm = norm,
		.va = vec2_dot(pa->vel, norm),
		.vb = vec2_dot(pb->vel, norm),
	};
}

void physics_bounce(unsigned a, unsigned b, physics_sep_t sep)
{
	physics_t *pa = physics_get(a);
	physics_t *pb = physics_get(b);

	const float ma = pa->mass;
	const float mb = pb->mass;
	const float ms = ma + mb;
	const float va = (sep.va * (ma - mb) + 2 * mb * sep.vb) / ms;
	const float vb = (sep.vb * (mb - ma) + 2 * ma * sep.va) / ms;

	pa->vel = vec2_add(pa->vel, vec2_scale(sep.norm, va - sep.va));
	pb->vel = vec2_add(pb->vel, vec2_scale(sep.norm, vb - sep.vb));
}