#include <cstdlib>
#include <cmath>
#include <GL/gl.h>

#include "util/Logger.hpp"
#include "util/convert.hpp"
#include "Geom.hpp"
#include "BSP.hpp"

BSP::BSP() {
	front = NULL;
	back = NULL;
}

void BSP::add_triangle(Vector a, Vector b, Vector c, void *material) {
	Triangle t = {a, b, c, material};
	soup.push_back(t);
}

void BSP::add_triangle(Triangle t) {
	soup.push_back(t);
}

int max(int a, int b) {
	return a > b ? a : b;
}

// less ratio - closer to a
Vector split(Vector a, Vector b, float ratio) {
	return a * (1 - ratio) + b * ratio;
}

#define EPSILON 0.005

#define INSIDE(D) ((-EPSILON <= (D)) && ((D) <= EPSILON))
#define BACK(D) ((D) < -EPSILON)
#define FRONT(D) ((D) > EPSILON)

void BSP::scatter_triangle(Triangle t) {
	float ad = dotProduct(t.a - origin, normal); // Signed distance
	float bd = dotProduct(t.b - origin, normal); // to the plane.
	float cd = dotProduct(t.c - origin, normal);

	#define III (INSIDE(ad) && INSIDE(bd) && INSIDE(cd)) // front
	#define IIF (INSIDE(ad) && INSIDE(bd) && FRONT(cd)) // front
	#define IIB (INSIDE(ad) && INSIDE(bd) && BACK(cd)) // back
	#define IFI (INSIDE(ad) && FRONT(bd) && INSIDE(cd)) // front
	#define IFF (INSIDE(ad) && FRONT(bd) && FRONT(cd)) // front
	#define IFB (INSIDE(ad) && FRONT(bd) && BACK(cd)) // split b-c
	#define IBI (INSIDE(ad) && BACK(bd) && INSIDE(cd)) // back
	#define IBF (INSIDE(ad) && BACK(bd) && FRONT(cd)) // split b-c
	#define IBB (INSIDE(ad) && BACK(bd) && BACK(cd)) // back
	#define FII (FRONT(ad) && INSIDE(bd) && INSIDE(cd)) // front
	#define FIF (FRONT(ad) && INSIDE(bd) && FRONT(cd)) // front
	#define FIB (FRONT(ad) && INSIDE(bd) && BACK(cd)) // split a-c
	#define FFI (FRONT(ad) && FRONT(bd) && INSIDE(cd)) // front
	#define FFF (FRONT(ad) && FRONT(bd) && FRONT(cd)) // front
	#define FFB (FRONT(ad) && FRONT(bd) && BACK(cd)) // split a-c, b-c
	#define FBI (FRONT(ad) && BACK(bd) && INSIDE(cd)) // split a-b
	#define FBF (FRONT(ad) && BACK(bd) && FRONT(cd)) // split a-b, b-c
	#define FBB (FRONT(ad) && BACK(bd) && BACK(cd)) // split a-b, a-c
	#define BII (BACK(ad) && INSIDE(bd) && INSIDE(cd)) // back
	#define BIF (BACK(ad) && INSIDE(bd) && FRONT(cd)) // split a-c
	#define BIB (BACK(ad) && INSIDE(bd) && BACK(cd)) // back
	#define BFI (BACK(ad) && FRONT(bd) && INSIDE(cd)) // split a-b
	#define BFF (BACK(ad) && FRONT(bd) && FRONT(cd)) // split a-b, a-c
	#define BFB (BACK(ad) && FRONT(bd) && BACK(cd)) // split a-b, b-c
	#define BBI (BACK(ad) && BACK(bd) && INSIDE(cd)) // back
	#define BBF (BACK(ad) && BACK(bd) && FRONT(cd)) // split a-c, b-c
	#define BBB (BACK(ad) && BACK(bd) && BACK(cd)) // back

	// Expand SPLIT(a, b) into split(t.a, t.b, ad / (ad - bd))
	// ad and bd always have opposite signs, so we do not need the abs().
	#define SPLIT(A, B) split(t.A, t.B, A ## d / (A ## d - B ## d))

	if (III || IIF || IFI || IFF || FII || FIF || FFI || FFF) {
		front->add_triangle(t);
		return;
	}

	if (IIB || IBI || IBB || BII || BIB || BBI || BBB) {
		back->add_triangle(t);
		return;
	}

	if (IFB) { // split b-c
		Vector point_on_bc = SPLIT(b, c);
		front->add_triangle(t.a, t.b, point_on_bc, t.material);
		back->add_triangle(t.a, t.c, point_on_bc, t.material);
		return;
	}

	if (IBF) { // split b-c
		Vector point_on_bc = SPLIT(b, c);
		back->add_triangle(t.a, t.b, point_on_bc, t.material);
		front->add_triangle(t.a, t.c, point_on_bc, t.material);
		return;
	}

	if (FIB) { // split a-c
		Vector point_on_ac = SPLIT(a, c);
		front->add_triangle(t.b, t.a, point_on_ac, t.material);
		back->add_triangle(t.b, t.c, point_on_ac, t.material);
		return;
	}

	if (FFB) { // split a-c, b-c
		Vector point_on_ac = SPLIT(a, c);
		Vector point_on_bc = SPLIT(b, c);
		front->add_triangle(t.a, t.b, point_on_bc, t.material);
		front->add_triangle(t.a, point_on_ac, point_on_bc, t.material);
		back->add_triangle(t.c, point_on_ac, point_on_bc, t.material);
		return;
	}

	if (FBI) { // split a-b
		Vector point_on_ab = SPLIT(a, b);
		front->add_triangle(t.c, t.a, point_on_ab, t.material);
		back->add_triangle(t.c, t.b, point_on_ab, t.material);
		return;
	}

	if (FBF) { // split a-b, b-c
		Vector point_on_ab = SPLIT(a, b);
		Vector point_on_bc = SPLIT(b, c);
		front->add_triangle(t.a, t.c, point_on_bc, t.material);
		back->add_triangle(t.b, point_on_ab, point_on_bc, t.material);
		front->add_triangle(t.a, point_on_ab, point_on_bc, t.material);
		return;
	}

	if (FBB) { // split a-b, a-c
		Vector point_on_ab = SPLIT(a, b);
		Vector point_on_ac = SPLIT(a, c);
		front->add_triangle(t.a, point_on_ab, point_on_ac, t.material);
		back->add_triangle(t.b, point_on_ab, point_on_ac, t.material);
		back->add_triangle(t.c, t.b, point_on_ac, t.material);
		return;
	}

	if (BIF) { // split a-c
		Vector point_on_ac = SPLIT(a, c);
		back->add_triangle(t.b, t.a, point_on_ac, t.material);
		front->add_triangle(t.b, t.c, point_on_ac, t.material);
		return;
	}

	if (BFI) { // split a-b
		Vector point_on_ab = SPLIT(a, b);
		back->add_triangle(t.c, t.a, point_on_ab, t.material);
		front->add_triangle(t.c, t.b, point_on_ab, t.material);
		return;
	}

	if (BFF) { // split a-b, a-c
		Vector point_on_ab = SPLIT(a, b);
		Vector point_on_ac = SPLIT(a, c);
		back->add_triangle(t.a, point_on_ab, point_on_ac, t.material);
		front->add_triangle(t.b, point_on_ab, point_on_ac, t.material);
		front->add_triangle(t.c, t.b, point_on_ac, t.material);
		return;
	}

	if (BFB) { // split a-b, b-c
		Vector point_on_ab = SPLIT(a, b);
		Vector point_on_bc = SPLIT(b, c);
		back->add_triangle(t.a, point_on_ab, point_on_bc, t.material);
		front->add_triangle(t.b, point_on_ab, point_on_bc, t.material);
		back->add_triangle(t.c, t.a, point_on_bc, t.material);
		return;
	}

	if (BBF) { // split a-c, b-c
		Vector point_on_ac = SPLIT(a, c);
		Vector point_on_bc = SPLIT(b, c);
		back->add_triangle(t.a, t.b, point_on_ac, t.material);
		back->add_triangle(t.b, point_on_bc, point_on_ac, t.material);
		front->add_triangle(t.c, point_on_bc, point_on_ac, t.material);
		return;
	}
}

// Recursively divides the soup into two groups (splitting triangles if
// needed), until the soup portions fit the soup_portion size.
// Returns the depth of the tree.
int BSP::subdivide(int soup_portion) {
	if (soup.size() <= soup_portion) {
		// no division needed
		return 0;
	}

	front = new BSP();
	back = new BSP();

	// choose the splitting plane
	int splitter_idx = std::rand() % soup.size();
	base = soup[splitter_idx];
	origin = base.a;
	normal = crossProduct(
		base.b - base.a,
		base.c - base.a
	).normalized();

	// distribute the triangles between front and back soups
	for (int i = 0; i < soup.size(); i++) {
		if (i == splitter_idx) {
			continue;
		}
		scatter_triangle(soup[i]);
	}
	soup.clear();

	return 1 + max(front->subdivide(soup_portion), back->subdivide(soup_portion));
}

Vector closest_point_on_triangle(Vector p0, Vector p1, Vector p2, Vector point) {
    Vector edge0 = p1 - p0;
    Vector edge1 = p2 - p0;
    Vector v0 = p0 - point;

    float a = dotProduct(edge0, edge0);
    float b = dotProduct(edge0, edge1);
    float c = dotProduct(edge1, edge1);
    float d = dotProduct(edge0, v0);
    float e = dotProduct(edge1, v0);

    float det = a * c - b * b;
    float s = b * e - c * d;
    float t = b * d - a * e;

    if (s + t < det) {
        if (s < 0) {
            if (t < 0) {
                if (d < 0) {
                    s = clamp(0, -d / a, 1);
                    t = 0;
                } else {
                    s = 0;
                    t = clamp(0, -e / c, 1);
                }
            } else {
                s = 0;
                t = clamp(0, -e / c, 1);
            }
        } else if (t < 0) {
            s = clamp(0, -d / a, 1);
            t = 0;
        } else {
            float invDet = 1 / det;
            s *= invDet;
            t *= invDet;
        }
    } else {
        if (s < 0) {
            float tmp0 = b + d;
            float tmp1 = c + e;
            if (tmp1 > tmp0) {
                float numer = tmp1 - tmp0;
                float denom = a - 2 * b + c;
                s = clamp(0, numer / denom, 1);
                t = 1 - s;
            } else {
                t = clamp(0, -e / c, 1);
                s = 0;
            }
        } else if (t < 0) {
            if (a + d > b + e) {
                float numer = c + e - b - d;
                float denom = a - 2 * b + c;
                s = clamp(0, numer / denom, 1);
                t = 1 - s;
            } else {
                s = clamp(0, -e / c, 1);
                t = 0;
            }
        } else {
            float numer = c + e - b - d;
            float denom = a - 2 * b + c;
            s = clamp(0, numer / denom, 1);
            t = 1 - s;
        }
    }

    return p0 + s * edge0 + t * edge1;
}

// Checks that the angle between triangles (a, b, c1) and (a, b, c2) is sharp.
// This means that points c1 and c2 are standing more or less on the same side
// from the "ab" vector.
bool sharp_angle_between_planes(Vector a, Vector b, Vector c1, Vector c2) {
        // (AB × AC1) · (AB × AC2) > 0
        return dotProduct(
                crossProduct(b - a, c1 - a),
                crossProduct(b - a, c2 - a)
        ) > 0;
}

// Checks that the point is to the inside from each edge of the triangle.
bool point_in_triangle(Vector a, Vector b, Vector c, Vector point) {
        return sharp_angle_between_planes(a, b, c, point)
                && sharp_angle_between_planes(b, c, a, point)
                && sharp_angle_between_planes(a, c, b, point);
}


// Finds out if a unit sphere would hit the triangles transformed by "model"
// matrix. On hit fills in the "push" vector which shows how to push the sphere
// out of the nearest triangle. Increments the "count" with each triangle
// tested.
bool BSP::unit_sphere_hits(Matrix model, Vector center, Vector *push, int *count, std::vector<Triangle> *hitting_triangles) {
	if (front != NULL) {
		// Not a leaf node

		//Matrix normal_matrix = ~model.transposed();
		//for (int i = 0; i < 4; i++) {
		//	normal_matrix(i, 3) = 0;
		//	normal_matrix(3, i) = 0;
		//}
		//normal_matrix(3, 3) = 1;

		//Vector global_normal = (normal_matrix * normal).normalized();
		//Vector global_origin = model * origin;

		// Another way of calculating the normal and origin
		Triangle global_triangle = {model * base.a, model * base.b, model * base.c};
		Vector global_normal = crossProduct(
			global_triangle.b - global_triangle.a,
			global_triangle.c - global_triangle.a
		).normalized();
		Vector global_origin = global_triangle.a;

		// Signed distance
		float d = dotProduct(center - global_origin, global_normal);

		if (d < -1) {
			return back->unit_sphere_hits(model, center, push, count, hitting_triangles);
		}

		if (d > 1) {
			return front->unit_sphere_hits(model, center, push, count, hitting_triangles);
		}

		bool hit = false;
		Vector newpush;

		if (back->unit_sphere_hits(model, center, &newpush, count, hitting_triangles)) {
			if (!hit || (newpush.length2() > push->length2())) {
				*push = newpush;
			}
			hit = true;
		}

		if (front->unit_sphere_hits(model, center, &newpush, count, hitting_triangles)) {
			if (!hit || (newpush.length2() > push->length2())) {
				*push = newpush;
			}
			hit = true;
		}

		(*count)++;
		Vector closest = closest_point_on_triangle(
			model * base.a,
			model * base.b,
			model * base.c,
			center
		);
		Vector from_closest = center - closest;
		if (from_closest.length2() < 1) {
			Vector n = from_closest.normalized();
			newpush = n - from_closest;

			hitting_triangles->push_back(base);
			if (!hit || (newpush.length2() > push->length2())) {
				*push = newpush;
			}
			hit = true;
		}

		return hit;
	} else {
		// A leaf node

		*count += soup.size();
		bool hit = false;
		for (int i = 0; i < soup.size(); i++) {
			Vector closest = closest_point_on_triangle(
				model * soup[i].a,
				model * soup[i].b,
				model * soup[i].c,
				center
			);
			Vector from_closest = center - closest;
			if (from_closest.length2() < 1) {
				Vector n = from_closest.normalized();
				Vector newpush = n - from_closest;

				hitting_triangles->push_back(base);
				if (!hit || (newpush.length2() > push->length2())) {
					*push = newpush;
				}
				hit = true;
			}
		}

		return hit;
	}
}

// Finds out if a "from-to" line would hit the triangles transformed by "model"
// matrix. On hit fills in the "hit" vector which shows where the line first
// hit some triangle.  Increments the "count" with each triangle tested.
bool BSP::line_hits(Matrix model, Vector from, Vector to, Vector *hit, void **material, int *count) {
	if (front != NULL) {
		// Not a leaf node

		Triangle global_triangle = {model * base.a, model * base.b, model * base.c};
		Vector global_normal = crossProduct(
			global_triangle.b - global_triangle.a,
			global_triangle.c - global_triangle.a
		).normalized();
		Vector global_origin = global_triangle.a;

		// Signed distance
		float dfrom = dotProduct(from - global_origin, global_normal);
		float dto = dotProduct(to - global_origin, global_normal);

		// Check if it hits before the plane
		if (dfrom < 0) {
			if (back->line_hits(model, from, to, hit, material, count)) {
				return true;
			}
		} else {
			if (front->line_hits(model, from, to, hit, material, count)) {
				return true;
			}
		}

		if (dfrom * dto < 0) {
			// The line goes through the plane
			Vector point_in_plane = split(from, to, dfrom / (dfrom - dto));
			(*count)++;

			// Check if it hits in the plane
			if (point_in_triangle(
				global_triangle.a,
				global_triangle.b,
				global_triangle.c,
				point_in_plane
			)) {
				// The line goes though the base triangle
				*hit = point_in_plane;
				*material = base.material;
				return true;
			}

			// Check if it hits after the plane
			if (dto < 0) {
				if (back->line_hits(model, from, to, hit, material, count)) {
					return true;
				}
			} else {
				if (front->line_hits(model, from, to, hit, material, count)) {
					return true;
				}
			}
		}

		return false;
	} else {
		// A leaf node
		*count += soup.size();

		bool was_hit = false;
		for (int i = 0; i < soup.size(); i++) {
			Vector a = model * soup[i].a;
			Vector b = model * soup[i].b;
			Vector c = model * soup[i].c;

			// Signed distance
			float dfrom = dotProduct(from - a, crossProduct(c - a, b - a));
			float dto = dotProduct(to - a, crossProduct(c - a, b - a));

			if (dfrom * dto < 0) {
				Vector point_in_plane = split(from, to, dfrom / (dfrom - dto));

				// Check if it hits in the plane
				if (point_in_triangle(a, b, c, point_in_plane)) {
					// The line goes though the triangle
					if (was_hit) {
						if ((*hit - from).length2() > (point_in_plane - from).length2()) {
							// The new intersection point is closer than the last one
							*hit = point_in_plane;
							*material = soup[i].material;
						}
					} else {
						*hit = point_in_plane;
						*material = soup[i].material;
						was_hit = true;
					}
				}
			}
		}

		return was_hit;
	}
}

bool same_triangles(Triangle a, Triangle b) {
	return ((a.a - b.a).length2() < EPSILON)
		&& ((a.b - b.b).length2() < EPSILON)
		&& ((a.c - b.c).length2() < EPSILON);
}

bool triangle_in_vector(Triangle a, std::vector<Triangle> *v) {
	for (int i = 0; i < v->size(); i++) {
		if (same_triangles(a, (*v)[i])) {
			return true;
		}
	}
	return false;
}

void BSP::render(Matrix model, Vector eyepos, float range) {
	glEnable(GL_DEPTH_TEST);
	if (front != NULL) {
		Triangle global_triangle = {model * base.a, model * base.b, model * base.c};
		Vector global_normal = crossProduct(
			global_triangle.b - global_triangle.a,
			global_triangle.c - global_triangle.a
		).normalized();
		Vector global_origin = global_triangle.a;

		// Signed distance
		float d = dotProduct(eyepos - global_origin, global_normal);

		if (fabsf(d) < range) {
			glBegin(GL_TRIANGLES);
				glColor3f(((size_t)this % 97) / 97.0 / 2 + 0.5, ((size_t)this % 1243) / 1243 / 2 + 0.5, ((size_t)this % 243) / 243.0 / 2 + 0.5);
				glVertex3f(base.a.x, base.a.y, base.a.z);
				glColor3f(((size_t)this % 123) / 123.0 / 2 + 0.5, ((size_t)this % 1321) / 1321 / 2 + 0.5, ((size_t)this % 243) / 243.0 / 2 + 0.5);
				glVertex3f(base.b.x, base.b.y, base.b.z);
				glColor3f(((size_t)this % 54) / 54.0 / 2 + 0.5, ((size_t)this % 625) / 625 / 2 + 0.5, ((size_t)this % 572) / 572.0 / 2 + 0.5);
				glVertex3f(base.c.x, base.c.y, base.c.z);
			glEnd();
		}
		if (d > -range) {
			front->render(model, eyepos, range);
		}
		if (d < range) {
			back->render(model, eyepos, range);
		}
	} else {
		glBegin(GL_TRIANGLES);
		for (int i = 0; i < soup.size(); i++) {
			glColor3f(((size_t)this % 97) / 97.0 / 2, ((size_t)this % 1243) / 1243.0 / 2, ((size_t)this % 243) / 243.0 / 2);
			glVertex3f(soup[i].a.x, soup[i].a.y, soup[i].a.z);
			glColor3f(((size_t)this % 123) / 123.0 / 2, ((size_t)this % 1321) / 1321 / 2, ((size_t)this % 243) / 243.0 / 2);
			glVertex3f(soup[i].b.x, soup[i].b.y, soup[i].b.z);
			glColor3f(((size_t)this % 54) / 54.0 / 2, ((size_t)this % 625) / 625 / 2, ((size_t)this % 572) / 572.0 / 2);
			glVertex3f(soup[i].c.x, soup[i].c.y, soup[i].c.z);
		}
		glEnd();
	}
}

BSP::~BSP() {
	if (front != NULL) {
		delete front;
	}
	if (back != NULL) {
		delete back;
	}
}