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

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

#include <algorithm>
#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);
	r->vert_count = vert_count;
	r->face_count = face_count;
	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
		));
	}
}

v2f Int_Collider::Projection::Iter::operator*() {
	v3f& pos = p->col->verts[vert];
	return v2f(v3f::dot(p->r, pos), v3f::dot(p->u, pos));
}

Int_Collider::Projection Int_Collider::project(const v3f& axis) const {
	v3f r = v3f::perp(axis);
	v3f u = v3f::cross(r, axis);
	return Projection { this, r, u };
}

struct Shrinkwrap {
	v2f* points;
	int count;
	Shrinkwrap(Arena* a, v2f* src, int src_count):
		count(0) {
		int i, top = 0;
		int stack_size = src_count;
		v2f p0(INFINITY, INFINITY);
		v2f* stack = (v2f*)arena_alloc(a, stack_size * sizeof *points);
		points = (v2f*)arena_alloc(a, src_count * sizeof *points);
		auto push = [&](v2f p) {
			assert(top < stack_size);
			stack[top++] = p;
		};
		auto pop = [&]() {
			assert(top);
			return stack[--top];
		};
		auto peek = [&](int i) {
			assert(top - 1 - i >= 0);
			return stack[top - 1 - i];
		};
		auto ccw = [](v2f a, v2f b, v2f c) {
			float angle =
				(b.y - a.y) * (c.x - b.x) -
				(b.x - a.x) * (c.y - b.y);
			return angle > -0.00001f;
		};
		for (i = 0; i < src_count; i++) {
			v2f p = src[i];
			if (p.y < p0.y || (p.y == p0.y && p.x < p0.x))
				p0 = p;
		}
		std::sort(
			src,
			src + src_count,
			[p0](const v2f& f, const v2f& s) {
				float a =
					(f.y - p0.y) * (s.x - f.x) -
					(f.x - p0.x) * (s.y - f.y);
				bool colinear = fabsf(a) < 0.001f;
				if (colinear)
					return v2f::mag(p0 - s) >= v2f::mag(p0 - f);
				return a < 0.0f;
			}
		);
		for (i = 0; i < src_count; i++) {
			v2f p = src[i];
			while (top > 1 && ccw(peek(1), peek(0), p))
				pop();
			push(p);
		}
		while (top)
			points[count++] = pop();
	}

	std::pair<v2f*, int> normals(Arena* a) const {
		int c = (count >> 1) + 1, i;
		v2f* normals = (v2f*)arena_alloc(a, c * sizeof *normals);
		for (i = 0; i < count; i++) {
			v2f a = points[i];
			v2f b = points[(i + 1) % count];
			v2f e = a - b;
			v2f n = v2f::normalised(v2f(e.y, -e.x));
			normals[i] = n;
		}
		return { normals, i };
	}

	bool SAT(Arena* a, const Shrinkwrap& other) {
		int i;
		auto[fn, fnc] = normals(a);
		auto[sn, snc] = other.normals(a);
		auto project = [](const Shrinkwrap& shape, v2f axis) {
			int i;
			float mini =  INFINITY;
			float maxi = -INFINITY;
			for (i = 0; i < shape.count; i++) {
				const v2f& p = shape.points[i];
				float d = v2f::dot(p, axis);
				if (d < mini) mini = d;
				if (d > maxi) maxi = d;
			}
			return std::pair<float, float>{ mini, maxi };
		};
		for (i = 0; i < fnc; i++) {
			v2f axis = fn[i];
			auto [fp1, fp2] = project(*this, axis);
			auto [sp1, sp2] = project(other, axis);
			if (fp2 > sp1 && fp1 > sp2)
				return true;
		}
		for (i = 0; i < snc; i++) {
			v2f axis = sn[i];
			auto [fp1, fp2] = project(*this, axis);
			auto [sp1, sp2] = project(other, axis);
			if (fp2 > sp1 && fp1 > sp2)
				return true;
		}
		return false;
	}
};

bool Int_Collider::collide(
	Physics_Debughook* hook,
	Arena* a,
	const Int_Collider& other
) const {
	auto check_axes = [&](
		const Int_Collider& f,
		const Int_Collider& s
	) {
		int i, c = s.face_count;
		v2f* fp, * sp;
		arena_push(a);
		fp = (v2f*)arena_alloc(a, sizeof *fp * f.vert_count);
		sp = (v2f*)arena_alloc(a, sizeof *sp * s.vert_count);
		for (i = 0; i < c; i++) {
			const v3f& axis = s.faces[i];
			int fpc = 0, spc = 0;
			for (v2f p : f.project(axis))
				fp[fpc++] = p;
			for (v2f p : s.project(axis))
				sp[spc++] = p;
			Shrinkwrap fwf(a, fp, fpc);
			Shrinkwrap swf(a, sp, spc);
#ifdef DEBUG
			if (hook) {
				hook->projection(&s, axis, i, sp, spc);
				hook->projection(&f, axis, i, fp, fpc);
				hook->shrinkwrap(&s, i, swf.points, swf.count);
				hook->shrinkwrap(&f, i, fwf.points, fwf.count);
			}
#endif
			if (fwf.SAT(a, swf)) return true;
		}
		arena_pop(a);
		return false;
	};
	if (check_axes(*this, other)) return false;
	if (check_axes(other, *this)) return false;
	(void)hook;
	return true;
}

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(Physics_Debughook* hook) {
		Int_Collider* a;
		for (a = bodies; a; a = a->next) {
			Int_Collider* b;
			for (b = a->next; b; b = b->next) {
				if (a->collide(hook, arena, *b)) {
					a->rb->colliding = 1;
					b->rb->colliding = 1;
				}
			}
		}
	}
};

void physics_update(
	World& w,
	Arena* a,
	float ts,
	Physics_Debughook* hook
) {
	Collision_Engine ce;
	ce.init(w, a);
	ce.detect(hook);
	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);
	}
}

void Physics_Debughook::shrinkwrap(
	const Int_Collider* col,
	int axis,
	const v2f* points,
	int count
) {
	(void)col;
	(void)axis;
	(void)points;
	(void)count;
}

void Physics_Debughook::projection(
	const Int_Collider* col,
	const v3f& n,
	int axis,
	const v2f* points,
	int count
) {
	(void)col;
	(void)n;
	(void)axis;
	(void)points;
	(void)count;
}