#undef NDEBUG
#include <cassert>
#include <core/object.h>
#include <scene/3d/immediate_geometry.h>
#include "wedge.h"
#include "base_data.h"
#include "contours.h"

Contours *Contours::singleton = nullptr;

Contours *Contours::get_singleton()
{
	if (!singleton)
		singleton = memnew(Contours);
	return singleton;
}

Contours::Contours()
	: dbg(nullptr)
{
	BaseData::get_singleton()->get().component<Lot>();
}

Contours::~Contours()
{
}

bool Contours::is_in_closed_contour(const struct wedge *w)
{
	int i, j;
	bool found = false;
	for (i = 0; i < (int)wedge_contours.size(); i++)
		for (j = 0; j < (int)wedge_contours[i].size(); j++)
			if (wedge_contours[i][j] == w)
				return true;
	return false;
}

static Vector3 tangent(const Vector3 &v)
{
	Vector2 rv = Vector2(v.x, v.z).tangent();
	return Vector3(rv.x, v.y, rv.y);
}
static Vector3 normal(const Vector3 &v)
{
	Vector3 rv = tangent(v);
	rv.y = 0.0f;
	return rv.normalized();
}

void Contours::build()
{
	int i, j;
	contours.clear();
	for (i = 0; i < (int)wedge_contours.size(); i++) {
		struct main_contour mc;
		for (j = 0; j < (int)wedge_contours[i].size(); j++) {
			struct wedge *w = wedge_contours[i][j];
			struct contour_wedge cw;
			Vector3 d1 = (w->p[1] - w->p[0]).normalized();
			Vector3 n1 = normal(d1);
			Vector3 d2 = (w->p[2] - w->p[1]).normalized();
			Vector3 n2 = normal(d2);
			Vector3 m1 = n1 * (w->width1);
			Vector3 m2 = n2 * (w->width2);
			Vector3 dx = (d1 + d2).normalized();
			Vector3 f1 = w->p[1] + m1;
			Vector3 f2 = w->p[1] + m2;
			Vector3 q, r;
			Geometry::get_closest_points_between_segments(
				w->p[0] + m1, f1, f2, w->p[2] + m2, q, r);
			r = q.linear_interpolate(r, 0.5f);
			cw.p[0] = w->p[0] + m1;
			cw.p[1] = r;
			cw.p[2] = w->p[2] + m2;
			if (mc.aabb.size.is_equal_approx(Vector3()))
				mc.aabb.position = cw.p[0];
			else
				mc.aabb.expand_to(cw.p[0]);
			mc.aabb.expand_to(cw.p[1]);
			mc.aabb.expand_to(cw.p[2]);
			mc.points.push_back(cw.p[0]);
			mc.points.push_back(cw.p[1]);
			cw.wedge = w;
			mc.contour_wedges.push_back(cw);
		}
		contours.push_back(mc);
	}
	flecs::entity base_e = BaseData::get_singleton()->get().lookup("lots");
	if (base_e.is_valid())
		base_e.destruct();
	base_e = BaseData::get_singleton()->get().entity("lots");
	List<struct Lot::polygon> polygon_queue;
	List<struct Lot::polygon> polygon_output;
	Vector<struct Lot::polygon> check;
	check.resize(contours.size());
	/* checking contours do not intersect */
	for (i = 0; i < (int)contours.size(); i++) {
		check.write[i] = { contours[i].points, contours[i].aabb };
	}
	Set<int> ignore;
	int k, l;
	for (i = 0; i < (int)contours.size(); i++) {
		for (j = 0; j < (int)check.size(); j++) {
			if (i != j) {
				for (k = 0; k < (int)contours[i].points.size();
				     k++) {
					for (l = 0;
					     l < (int)contours[j].points.size();
					     l++) {
						assert(!contours[i].points[k].is_equal_approx(
							contours[j].points[l]));
					}
				}
			}
		}
	}
	for (i = 0; i < (int)contours.size(); i++) {
		check.write[i] = { contours[i].points, contours[i].aabb };
		check.write[i].update_aabb();
	}
	for (i = 0; i < check.size(); i++) {
		for (j = 0; j < (int)check.size(); j++) {
			if (i == j)
				continue;
			check[i].intersects(&check[j]);
		}
	}
	for (i = 0; i < (int)check.size(); i++) {
		polygon_queue.push_back(check[i]);
#if 0
			polygon_output.push_back(check[i]);
#endif
	}
#if 0
	for (i = 0; i < (int)contours.size(); i++)
		polygon_queue.push_back(
			{ contours[i].points, contours[i].aabb });
#endif
	{
		List<struct Lot::polygon>::Element *e = polygon_queue.front();
		while (e) {
			struct Lot::polygon item = e->get();
			print_line("queue: " + itos(polygon_queue.size()));
			polygon_queue.pop_front();
			float area = item.area();
			if (area > 120.0f * 120.0f &&
			    item.aabb.size.x > 10.0f &&
			    item.aabb.size.z > 10.0f) {
				std::pair<struct Lot::polygon,
					  struct Lot::polygon>
					polys = item.split();
				if (polys.first.area() < 10.0f ||
				    polys.second.area() < 10.0f) {
					polygon_output.push_back(item);
				} else {
					polygon_queue.push_back(polys.first);
					polygon_queue.push_back(polys.second);
				}
			} else
				polygon_output.push_back(item);
			e = polygon_queue.front();
		}
		e = polygon_output.front();
		while (e) {
			flecs::entity lot_e = BaseData::get_singleton()
						      ->get()
						      .entity()
						      .child_of(base_e);
			lot_e.set<Lot>({ e->get() });
			e = e->next();
		}
	}
	BaseData::get_singleton()->get().query_builder<Lot>().build().each(
		[&](flecs::entity e, Lot &lot) { lot.polygon.update_aabb(); });
	BaseData::get_singleton()->get().query_builder<Lot>().build().each(
		[&](flecs::entity e, Lot &lot) {
			BaseData::get_singleton()
				->get()
				.query_builder<Lot>()
				.build()
				.each([&](flecs::entity we, Lot &wlot) {
					if (e != we) {
						assert(!lot.polygon.intersects(
							&wlot.polygon));
					}
				});
		});
	Lot::pack();
}

void Contours::debug()
{
	return;
	int i, j;
	if (!dbg) {
		dbg = memnew(ImmediateGeometry);
		SceneTree::get_singleton()->get_current_scene()->call_deferred(
			"add_child", dbg);
		if (!imm_mat.is_valid()) {
			imm_mat.instance();
			imm_mat->set_flag(
				SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR,
				true);
			imm_mat->set_flag(
				SpatialMaterial::FLAG_DISABLE_DEPTH_TEST, true);
			imm_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
		}
		dbg->set_material_override(imm_mat);
	}
	dbg->clear();
	dbg->begin(Mesh::PRIMITIVE_LINES);
	dbg->set_color(Color(1, 1, 0, 1));
	for (i = 0; i < (int)wedge_contours.size(); i++) {
		for (j = 0; j < (int)wedge_contours[i].size(); j++) {
			dbg->add_vertex(wedge_contours[i][j]->p[0] +
					Vector3(0, 1, 0));
			dbg->add_vertex(wedge_contours[i][j]->p[1] +
					Vector3(0, 1, 0));
			dbg->add_vertex(wedge_contours[i][j]->p[1] +
					Vector3(0, 1, 0));
			dbg->add_vertex(wedge_contours[i][j]->p[2] +
					Vector3(0, 1, 0));
		}
	}
	dbg->end();
	flecs::entity base_e = BaseData::get_singleton()->get().lookup("lots");
	assert(base_e.is_valid());
	flecs::query<Lot> query = BaseData::get_singleton()
					  ->get()
					  .query_builder<Lot>()
					  .with(flecs::ChildOf, base_e)
					  .build();
	dbg->begin(Mesh::PRIMITIVE_LINES);
	dbg->set_color(Color(1, 1, 0.4f, 1));
	query.each([&](flecs::entity e, const Lot &lot) {
		for (j = 0; j < (int)lot.polygon.points.size(); j++) {
			dbg->add_vertex(lot.polygon.points[j] +
					Vector3(0, 2, 0));
			dbg->add_vertex(
				lot.polygon.points[(j + 1) %
						   lot.polygon.points.size()] +
				Vector3(0, 2, 0));
			List<Pair<AABB, String> > lot_data;
			Lot::get_lot_data(e, &lot_data);
			List<Pair<AABB, String> >::Element *me =
				lot_data.front();
			while (me) {
				const Pair<AABB, String> &data = me->get();
				dbg->add_vertex(data.first.get_center());
				dbg->add_vertex(data.first.get_center() +
						Vector3(0, 50, 0));
				me = me->next();
			}
		}
	});
	dbg->end();
	dbg->begin(Mesh::PRIMITIVE_LINES);
	dbg->set_color(Color(1, 1, 0.3, 1));
	query.each([&](flecs::entity e, const Lot &lot) {
		for (i = 0; i < lot.polygon.intersections.size(); i++) {
			for (j = 0; j < lot.polygon.intersections[i].size();
			     j++) {
				int i0 = j;
				int i1 = (j + 1) %
					 lot.polygon.intersections.size();
				Vector2 p0 = lot.polygon.intersections[i][i0];
				Vector2 p1 = lot.polygon.intersections[i][i1];
				dbg->add_vertex(Vector3(p0.x, 50, p0.y));
				dbg->add_vertex(Vector3(p1.x, 50, p1.y));
			}
			assert(false);
		}
	});
	dbg->end();
	dbg->begin(Mesh::PRIMITIVE_LINES);
	dbg->set_color(Color(0.8f, 0.5f, 0.9f, 1));
	query.each([&](flecs::entity e, const Lot &lot) {
		Vector<Pair<Rect2, String> > buildings;
		Lot::get_lot_buildings(e, &buildings);
		Vector<Vector<Vector2> > polys = Lot::get_lot_polygons(e);
		for (i = 0; i < (int)buildings.size(); i++) {
			const Rect2 r = buildings[i].first;
			Vector2 mp0 = r.position;
			Vector2 mp1 = r.position + Vector2(r.size.x, 0);
			Vector2 mp2 = r.position + r.size;
			Vector2 mp3 = r.position + Vector2(0, r.size.y);
			Vector2 rect[] = { mp0, mp1, mp2, mp3 };
			Vector2 c = r.get_center();
			dbg->set_color(Color(0.3f, 0.1f, 0.9f, 1));
			for (j = 0; j < 4; j++) {
				int i0 = j;
				int i1 = (j + 1) % 4;
				Vector3 p0 = Vector3(rect[i0].x, 0, rect[i0].y);
				Vector3 p1 = Vector3(rect[i1].x, 0, rect[i1].y);
				dbg->add_vertex(p0);
				dbg->add_vertex(p1);
				dbg->add_vertex(p0 + Vector3(0, -10, 0));
				dbg->add_vertex(p0 + Vector3(0, +10, 0));
			}
#if 0
			dbg->set_color(Color(0.1f, 0.1f, 1.0f, 1));
			dbg->add_vertex(Vector3(c.x, 0, c.y));
			dbg->add_vertex(lot.polygon.points[0]);
#endif
			dbg->set_color(Color(0.3f, 0.1f, 0.9f, 1));
			assert(polys[i].size() > 0);
			for (j = 0; j < (int)polys[i].size(); j++) {
				int i0 = j;
				int i1 = (j + 1) % (int)polys[i].size();
				Vector3 p0 = Vector3(polys[i][i0].x, 100,
						     polys[i][i0].y);
				Vector3 p1 = Vector3(polys[i][i1].x, 100,
						     polys[i][i1].y);
				dbg->add_vertex(p0);
				dbg->add_vertex(p1);
			}
			dbg->set_color(Color(1.0f, 0.1f, 0.1f, 1));
			dbg->add_vertex(Vector3(c.x, 0, c.y));
			dbg->add_vertex(
				Vector3(polys[i][0].x, 100, polys[i][0].y));
		}
	});
	dbg->end();
}