#include "debugdraw.hpp"
#include "physics.hpp"
#include "scene.hpp"
#include "world.hpp"

extern "C" {
#include "memory.h"
#include "str.h"
}

#include <new>

void Collider::debug_render(
	Line_Renderer& lr,
	const m4f& t,
	const v3f& col
) {
	int i, c = face_count;
	for (i = 0; i < c; i++) {
		Face& f = faces[i];
		uint16_t prev = f.verts[0];
		v3f avg = (t * v4f(verts[prev], 1.0f)).xyz();
		int j;
		lr.colour(col);
		for (j = 1; j < f.vert_count; j++) {
			uint16_t idx = f.verts[j];
			v3f from = (t * v4f(verts[prev], 1.0f)).xyz();
			v3f to = (t * v4f(verts[idx], 1.0f)).xyz();
			lr.add_line(from, to);
			avg += to;
			prev = idx;
		}
		lr.add_line(
			(t * v4f(verts[prev], 1.0f)).xyz(),
			(t * v4f(verts[f.verts[0]], 1.0f)).xyz()
		);
		avg /= (float)f.vert_count;
		lr.colour(v3f(0.3f, 1.0f, 1.0f));
		lr.add_arrow(avg, avg + (t * v4f(f.normal, 0.0f)).xyz());
	}
}

Int_Collider* Collider::transform(const m4f& t, Arena* a) const {
	int i;
	Int_Collider* r = (Int_Collider*)arena_alloc(a, sizeof *r);
	r->faces = (v3f*)arena_alloc(a, face_count * sizeof *r->faces);
	r->verts = (v3f*)arena_alloc(a, vert_count * sizeof *r->verts);
	for (i = 0; i < face_count; i++)
		r->faces[i] = (t * v4f(faces[i].normal, 0.0f)).xyz();
	for (i = 0; i < vert_count; i++)
		r->verts[i] = (t * v4f(verts[i], 1.0f)).xyz();
	return r;
}

Collider* make_box(Arena* a, const v3f& size) {
	int i;
	v3f he = size * 0.5f;
	v3f* verts;
	Collider::Face* faces;
	Collider* c = (Collider*)arena_alloc(a, sizeof *c);
	new(c) Collider();
	c->vert_count = 8;
	c->face_count = 6;
	verts = (v3f*)arena_alloc(
		a,
		sizeof *c->verts * c->vert_count
	);
	faces = (Collider::Face*)arena_alloc(
		a,
		sizeof *c->faces * c->face_count
	);
	for (i = 0; i < c->face_count; i++) {
		Collider::Face& f = faces[i];
		f.vert_count = 4;
		f.verts = (uint16_t*)arena_alloc(
			a,
			sizeof *f.verts * f.vert_count
		);
	}
	c->verts = verts;
	c->faces = faces;
	verts[0] = he;
	verts[1] = v3f( he.x,  he.y, -he.z);
	verts[2] = v3f(-he.x,  he.y, -he.z);
	verts[3] = v3f(-he.x,  he.y,  he.z);
	verts[4] = v3f( he.x, -he.y,  he.z);
	verts[5] = v3f( he.x, -he.y, -he.z);
	verts[6] = -he;
	verts[7] = v3f(-he.x, -he.y,  he.z);
	faces[0].normal = v3f( 1.0f,  0.0f,  0.0f);
	faces[0].verts[0] = 0;
	faces[0].verts[1] = 1;
	faces[0].verts[2] = 5;
	faces[0].verts[3] = 4;
	faces[1].normal = v3f(-1.0f,  0.0f,  0.0f);
	faces[1].verts[0] = 2;
	faces[1].verts[1] = 3;
	faces[1].verts[2] = 7;
	faces[1].verts[3] = 6;
	faces[2].normal = v3f( 0.0f,  1.0f,  0.0f);
	faces[2].verts[0] = 2;
	faces[2].verts[1] = 3;
	faces[2].verts[2] = 0;
	faces[2].verts[3] = 1;
	faces[3].normal = v3f( 0.0f, -1.0f,  0.0f);
	faces[3].verts[0] = 6;
	faces[3].verts[1] = 7;
	faces[3].verts[2] = 4;
	faces[3].verts[3] = 5;
	faces[4].normal = v3f( 0.0f,  0.0f,  1.0f);
	faces[4].verts[0] = 4;
	faces[4].verts[1] = 7;
	faces[4].verts[2] = 3;
	faces[4].verts[3] = 0;
	faces[5].normal = v3f( 0.0f,  0.0f, -1.0f);
	faces[5].verts[0] = 6;
	faces[5].verts[1] = 5;
	faces[5].verts[2] = 1;
	faces[5].verts[3] = 2;
	c->name = dup_string(a, "<box>");
	c->loader = 0;
	{
		v3f i, hl2 = he * he;
		float a = 1.0f / 12.0f;
		i.x = a * (hl2.z + hl2.y);
		i.y = a * (hl2.x + hl2.z);
		i.z = a * (hl2.x + hl2.y);
		c->moment = m3f(
			v3f(i.x,  0.0f, 0.0f),
			v3f(0.0f, i.y,  0.0f),
			v3f(0.0f, 0.0f, i.z)
		).inverse();
	}
	return c;
}

void Rigidbody::init(
	Collider* c,
	const v3f& p,
	const v4f& r,
	float mass
) {
	col = c;
	set_pos(p);
	set_rot(r);
	set_mass(mass);
	force = v3f(0.0f);
	torque = v3f(0.0f);
	vel = v3f(0.0f);
	avel = v3f(0.0f);
	set_mass(mass);
}

void Rigidbody::set_pos(const v3f& p) {
	pos = p;
	prev_pos = p;
}

void Rigidbody::set_rot(const v4f& r) {
	rot = r;
	prev_rot = r;
}

void Rigidbody::set_mass(float m) {
	if (m > 0.000001f)
		inv_mass = 1.0f / m;
	else
		inv_mass = 0.0f;
}

void Rigidbody::add_force(const v3f& f) {
	force += f;
}

void Rigidbody::add_force(const v3f& f, const v3f& p) {
	force += f;
	add_torque(v3f::cross(f, p));
}

void Rigidbody::add_impulse(const v3f& f) {
	vel += f * inv_mass;
}

void Rigidbody::add_impulse(const v3f& f, const v3f& p) {
	vel  += f * inv_mass;
	avel += v3f::cross(f, p) * inv_mass;
}

void Rigidbody::add_torque(const v3f& t) {
	torque += t;
}

void physics_tick(World& w, float ts) {
	for (auto v : w.view<Rigidbody>()) {
		auto& rb = v.get<Rigidbody>();
		v3f accel = rb.inv_mass * rb.force;
		v3f aaccel = rb.col->moment * rb.torque;
		rb.prev_pos = rb.pos;
		rb.prev_rot = rb.rot;
		rb.vel += accel * ts;
		rb.avel += aaccel * ts;
		rb.pos += rb.vel * ts;
		rb.rot = quat::normalised(rb.rot + quat::mul<false>(
			quat::scale(v4f(rb.avel * 0.5f, 0.0f), ts),
			rb.rot
		));
	}
}

bool Int_Collider::collide(const Int_Collider& other) const {
	return false;
}

struct Collision_Engine {
	Int_Collider* bodies;
	Arena* arena;
	void init(World& w, Arena* a) {
		arena = a;
		bodies = 0;
		for (auto v : w.view<Transform, Rigidbody>()) {
			auto& t = v.get<Transform>();
			auto& r = v.get<Rigidbody>();
			Int_Collider* b = r.col->transform(t.mat, a);
			b->rb = &r;
			r.colliding = 0;
			b->next = bodies;
			bodies = b;
		}
	}
	void detect() {
		Int_Collider* a;
		for (a = bodies; a; a = a->next) {
			Int_Collider* b;
			for (b = a->next; b; b = b->next) {
				if (a->collide(*b)) {
					a->rb->colliding = 1;
					b->rb->colliding = 1;
				}
			}
		}
	}
};

void physics_update(World& w, Arena* a, float ts) {
	Collision_Engine ce;
	ce.init(w, a);
	ce.detect();
	physics_tick(w, ts);
	for (auto v : w.view<Transform, Rigidbody>()) {
		auto& t = v.get<Transform>();
		auto& r = v.get<Rigidbody>();
		t.mat =
			m4f::translate(m4f::identity(), r.pos) *
			m4f::rotate(m4f::identity(), r.rot);
	}
}

void physics_debug(World& w, Line_Renderer& r) {
	for (auto v : w.view<Transform, Rigidbody>()) {
		Transform& t = v.get<Transform>();
		Rigidbody& rb = v.get<Rigidbody>();
		Collider* c = rb.col;
		v3f col = rb.colliding?
			v3f(1.0f, 0.0f, 0.0f):
			v3f(0.3f, 1.0f, 0.3f);
		c->debug_render(r, t.mat, col);
		r.colour(v3f(1.0f, 0.3f, 0.3f));
		r.add_arrow(rb.pos, rb.pos + rb.vel);
		r.colour(v3f(0.3f, 0.3f, 1.0f));
		r.add_arrow(rb.pos, rb.pos + rb.avel);
		r.colour(v3f(1.0f, 1.0f, 0.3f));
		r.add_arrow(rb.pos, rb.pos + rb.torque);
	}
}