#include "meshcollider.hpp"
#include "std.hpp"

static const int      MAX_COLL_TRIS = 16;
static vector<Vector> tri_centres;

extern float stats3d[10];

extern gxRuntime* gx_runtime;

static bool triTest(const Vector a[3], const Vector b[3])
{
	bool  pb0 = false, pb1 = false, pb2 = false;
	Plane p(a[0], a[1], a[2]), p0, p1, p2;
	for (int k = 0; k < 3; ++k) {
		Line  l(b[k], b[(k + 1) % 3] - b[k]);
		float t = p.t_intersect(l);
		if (t < 0 || t > 1)
			continue;
		Vector i = l * t;
		if (!pb0) {
			p0  = Plane(a[0] + p.n, a[1], a[0]);
			pb0 = true;
		}
		if (p0.distance(i) < 0)
			continue;
		if (!pb1) {
			p1  = Plane(a[1] + p.n, a[2], a[1]);
			pb1 = true;
		}
		if (p1.distance(i) < 0)
			continue;
		if (!pb2) {
			p2  = Plane(a[2] + p.n, a[0], a[2]);
			pb2 = true;
		}
		if (p2.distance(i) < 0)
			continue;
		return true;
	}
	return false;
}

static bool trisIntersect(const Vector a[3], const Vector b[3])
{
	return triTest(a, b) || triTest(b, a);
}

MeshCollider::MeshCollider(const vector<Vertex>& verts, const vector<Triangle>& tris) : vertices(verts), triangles(tris)
{
	vector<int> ts;
	tri_centres.clear();
	for (int k = 0; k < triangles.size(); ++k) {
		const MeshCollider::Triangle& t  = triangles[k];
		const Vector&                 v0 = vertices[t.verts[0]].coords;
		const Vector&                 v1 = vertices[t.verts[1]].coords;
		const Vector&                 v2 = vertices[t.verts[2]].coords;
		tri_centres.push_back((v0 + v1 + v2) / 3);
		ts.push_back(k);
	}
	tree = createNode(ts);
}

MeshCollider::~MeshCollider()
{
	delete tree;
}

bool MeshCollider::collide(const Line& line, float radius, Collision* curr_coll, const Transform& t)
{
	if (!tree)
		return false;

	//create local box
	Box box(line);
	box.expand(radius);
	Box local_box = -t * box;

	return collide(local_box, line, radius, t, curr_coll, tree);
}

bool MeshCollider::collide(const Box& line_box, const Line& line, float radius, const Transform& tform,
						   Collision* curr_coll, MeshCollider::Node* node)
{
	if (!line_box.overlaps(node->box)) {
		return false;
	}

	bool hit = false;
	if (!node->triangles.size()) {
		if (node->left)
			hit |= collide(line_box, line, radius, tform, curr_coll, node->left);
		if (node->right)
			hit |= collide(line_box, line, radius, tform, curr_coll, node->right);
		return hit;
	}

	stats3d[0] += node->triangles.size();

	for (int k = 0; k < node->triangles.size(); ++k) {
		const Triangle& tri  = triangles[node->triangles[k]];
		const Vector&   t_v0 = vertices[tri.verts[0]].coords;
		const Vector&   t_v1 = vertices[tri.verts[1]].coords;
		const Vector&   t_v2 = vertices[tri.verts[2]].coords;

		//tri box
		Box tri_box(t_v0);
		tri_box.update(t_v1);
		tri_box.update(t_v2);
		if (!tri_box.overlaps(line_box))
			continue;

		if (!curr_coll->triangleCollide(line, radius, tform * t_v0, tform * t_v1, tform * t_v2))
			continue;

		curr_coll->surface = tri.surface;
		curr_coll->index   = tri.index;

		hit = true;
	}
	return hit;
}

Box MeshCollider::nodeBox(const vector<int>& tris)
{
	Box box;
	for (int k = 0; k < tris.size(); ++k) {
		const Triangle& t = triangles[tris[k]];
		for (int j = 0; j < 3; ++j)
			box.update(vertices[t.verts[j]].coords);
	}
	return box;
}

MeshCollider::Node* MeshCollider::createLeaf(const vector<int>& tris)
{
	Node* c      = new Node;
	c->box       = nodeBox(tris);
	c->triangles = tris;
	leaves.push_back(c);
	return c;
}

MeshCollider::Node* MeshCollider::createNode(const vector<int>& tris)
{
	if (tris.size() <= MAX_COLL_TRIS)
		return createLeaf(tris);

	Node* c = new Node;
	c->box  = nodeBox(tris);

	//find longest axis
	//
	float max = c->box.width();
	if (c->box.height() > max)
		max = c->box.height();
	if (c->box.depth() > max)
		max = c->box.depth();
	int axis = 0;
	if (max == c->box.height())
		axis = 1;
	else if (max == c->box.depth())
		axis = 2;

	//sort by axis
	//
	int                  k;
	multimap<float, int> axis_map;
	for (k = 0; k < tris.size(); ++k) {
		pair<float, int> p(tri_centres[tris[k]][axis], tris[k]);
		axis_map.insert(p);
	}

	//generate left node
	//
	vector<int>                    new_tris;
	multimap<float, int>::iterator it = axis_map.begin();
	for (k = axis_map.size() / 2; k--; ++it) {
		new_tris.push_back(it->second);
	}
	c->left = createNode(new_tris);

	//generate right node
	//
	new_tris.clear();
	for (; it != axis_map.end(); ++it) {
		new_tris.push_back(it->second);
	}
	c->right = createNode(new_tris);

	return c;
}

bool MeshCollider::intersects(const MeshCollider& c, const Transform& t) const
{
	static Vector a[MAX_COLL_TRIS][3], b[3];

	if (!(t * tree->box).overlaps(c.tree->box))
		return false;
	for (int k = 0; k < leaves.size(); ++k) {
		Node* p       = leaves[k];
		Box   box     = t * p->box;
		bool  tformed = false;
		for (int j = 0; j < c.leaves.size(); ++j) {
			Node* q = c.leaves[j];
			if (!box.overlaps(q->box))
				continue;
			if (!tformed) {
				for (int n = 0; n < p->triangles.size(); ++n) {
					const Triangle& tri = triangles[p->triangles[n]];
					a[n][0]             = t * vertices[tri.verts[0]].coords;
					a[n][1]             = t * vertices[tri.verts[1]].coords;
					a[n][2]             = t * vertices[tri.verts[2]].coords;
				}
				tformed = true;
			}
			for (int n = 0; n < q->triangles.size(); ++n) {
				const Triangle& tri = c.triangles[q->triangles[n]];
				b[0]                = c.vertices[tri.verts[0]].coords;
				b[1]                = c.vertices[tri.verts[1]].coords;
				b[2]                = c.vertices[tri.verts[2]].coords;
				for (int t = 0; t < p->triangles.size(); ++t) {
					if (trisIntersect(a[t], b))
						return true;
				}
			}
		}
	}
	return false;
}