#include <cassert>
#include <cmath>
#include <core/math/geometry.h>
#include <core/resource.h>
#include <scene/2d/canvas_item.h>
#include <modules/voronoi/voronoi.h>
#include <modules/opensimplex/open_simplex_noise.h>
#include "road_grid.h"

RoadGrid::RoadGrid()
{
	grid_width = 16;
	grid_height = 16;
	class_sizes[SITE_EMPTY] = 10000;
	class_sizes[SITE_TOWN] = 100000;
	class_sizes[SITE_FARM] = 500000;
	class_sizes[SITE_FOREST] = 1000000;
	class_sizes[SITE_UNASSIGNED] = 2000000;
}

RoadGrid::~RoadGrid()
{
}

/* TODO: constants, configuration */
Dictionary RoadGrid::build_diagram(int npatches, int center_count, int center_step, int spread, int dim)
{
	printf("build_diagram %d %d %d %d %d\n", npatches, center_count, center_step, spread, dim);
	Vector<Vector2i> centers;
	/* zero is always used */
	centers.push_back(Vector2i(0, 0));
	float sa = rnd->randf() * 2.0 * Math_PI;
	int i, bad = 0, cx, cp;
	while (centers.size() < center_count) {
		int center_x = CLAMP(center_step * (int)((rnd->randi() % spread) - spread / 2), -dim, dim);
		int center_y = CLAMP(center_step * (int)((rnd->randi() % spread) - spread / 2), -dim, dim);
		Vector2i c(center_x, center_y);
		if (centers.find(c) < 0) {
			centers.push_back(c);
			bad = 0;
		} else
			bad++;
		if (bad > 1000)
			break;
	}
	assert(centers.size() > 1);
	Vector<Vector2> points;
	for (cx = 0; cx < centers.size(); cx++) {
		float maxr = 0.0f;
		for (cp = 0; cp < npatches * 8; cp++) {
			float a = sa + sqrtf((float)cp) * 8.0f;
			float r = (cp == 0) ? 0.0f : 100.0f + (float)cp * 100.0f + 50.0f * rnd->randf();
			float x = floor(cosf(a) * r + (float)centers[cx].x);
			float y = floor(sinf(a) * r + (float)centers[cx].y);
			if (maxr < r)
				maxr = r;
			Vector2 p(x, y);
			if (points.find(p) < 0)
				points.push_back(p);
		}
		struct cluster cst;
		cst.c = centers[cx];
		cst.r = maxr + 50.0f;
		clusters.push_back(cst);
	}
	PoolVector<Vector2> cpoints;
	cpoints.resize(points.size());
	memcpy(cpoints.write().ptr(), points.ptr(), points.size() * sizeof(Vector2));

	Dictionary diagram = Voronoi::get_singleton()->generate_diagram(cpoints, 11);
	return diagram;
}
bool RoadGrid::segment_intersects_rect(const Vector2 &a, const Vector2 &b, const Rect2 &rect)
{
	real_t min = 0, max = 1;
	const Vector2 &p_from = a;
	const Vector2 &p_to = b;

	for (int i = 0; i < 2; i++) {
		real_t seg_from = p_from[i];
		real_t seg_to = p_to[i];
		real_t box_begin = rect.position[i];
		real_t box_end = box_begin + rect.size[i];
		real_t cmin, cmax;

		if (seg_from < seg_to) {
			if (seg_from > box_end || seg_to < box_begin) {
				return false;
			}
			real_t length = seg_to - seg_from;
			cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
			cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;

		} else {
			if (seg_to > box_end || seg_from < box_begin) {
				return false;
			}
			real_t length = seg_to - seg_from;
			cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
			cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
		}

		if (cmin > min) {
			min = cmin;
		}
		if (cmax < max) {
			max = cmax;
		}
		if (max < min) {
			return false;
		}
	}

	return true;
}

void RoadGrid::draw_debug(Node *drawable, int size_x, int size_y) const
{
	int i, j;
	CanvasItem *ci = Object::cast_to<CanvasItem>(drawable);
	if (!ci)
		return;
	if (bounds.size.x <= 0.0f || bounds.size.y <= 0.0f) {
		printf("zero bounds %f %f\n", bounds.size.x, bounds.size.y);
		return;
	}
	for (i = 0; i < map_sites.size(); i++) {
		Vector2 a = (map_sites[i].pos - bounds.position) * Vector2(size_x, size_y) / bounds.size;
		ci->draw_circle(a, 4.0f, Color(0.0f, 0.5f, 0.5f, 1.0f));
	}
	for (i = 0; i < map_hedges.size(); i++) {
		Vector2 xa = diagram_vertices[map_hedges[i]->a];
		Vector2 xb = diagram_vertices[map_hedges[i]->b];
		Vector2 a = (xa - bounds.position) * Vector2(size_x, size_y) / bounds.size;
		Vector2 b = (xb - bounds.position) * Vector2(size_x, size_y) / bounds.size;
		ci->draw_line(a, b, Color(0.0f, 0.7f, 0.7f, 1.0f), 3.0f, true);
	}
	Rect2i g = rect_to_grid(bounds);
	for (i = g.position.y - 1; i < g.position.y + g.size.y + 1; i++) {
		for (j = g.position.x - 1; j < g.position.x + g.size.x + 1; j++) {
			if (hedge_grid.has(j) && hedge_grid[j].has(i)) {
				List<struct half_edge *> items = hedge_grid[j][i];
				List<struct half_edge *>::Element *e;
				for (e = items.front(); e; e = e->next()) {
					struct half_edge *he = e->get();

					assert(he->a >= 0);
					assert(he->b >= 0);
					assert(he->a != he->b);
					assert(he->site >= 0);

					Vector2 xa = diagram_vertices[he->a];
					Vector2 xb = diagram_vertices[he->b];
					const struct map_site *site = &map_sites[he->site];
					Vector2 pos = (site->pos - bounds.position) * Vector2(size_x, size_y) / bounds.size;
					ci->draw_circle(pos, 3.0f, Color(0.2f, 0.2f, 0.5f, 1.0f));
					Vector2 a = (xa - bounds.position) * Vector2(size_x, size_y) / bounds.size;
					Vector2 b = (xb - bounds.position) * Vector2(size_x, size_y) / bounds.size;
					ci->draw_line(a, b, Color(0.2f, 0.2f, 0.7f, 1.0f), 2.0f, true);
				}
			}
		}
	}
}

void RoadGrid::index_site(struct map_site *site)
{
	int i;
	site->vertices_ind.resize(site->vertices.size());
	site->polygon_ind.resize(site->polygon.size());
	/* slow as fuck */
	for (i = 0; i < site->vertices.size(); i++) {
		Vector2 v = site->vertices[i].snapped(Vector2(2.0f, 2.0f));
		int idx = diagram_vertices.find(v);
		if (idx < 0) {
			idx = diagram_vertices.size();
			diagram_vertices.push_back(v);
		}
		site->vertices_ind.write[i] = idx;
	}
	for (i = 0; i < site->polygon.size(); i++) {
		Vector2 v = site->polygon[i].snapped(Vector2(2.0f, 2.0f));
		int idx = diagram_vertices.find(v);
		if (idx < 0) {
			idx = diagram_vertices.size();
			diagram_vertices.push_back(v);
		}
		site->polygon_ind.write[i] = idx;
	}
	site->hedges.resize(site->polygon.size());
	int count = 0;
	for (i = 0; i < site->polygon.size(); i++) {
		int idx1 = site->polygon_ind[i];
		int idx2 = site->polygon_ind[(i + 1) % site->polygon.size()];
		if (idx1 == idx2)
			continue;
		struct half_edge he;
		he.a = idx1;
		he.b = idx2;
		he.site = site->index;
		/* use length to decide */
		he.depth = 6.0f;
		he.length = diagram_vertices[idx1].distance_to(diagram_vertices[idx2]);
		site->hedges.write[count++] = he;
	}
	site->hedges.resize(count);
}

void RoadGrid::process_diagram(const Dictionary &diagram)
{
	const Array &sites = diagram["sites"];
	int i, j;
	map_sites.resize(sites.size());
	int hedge_count = 0, hedge_idx;
	printf("start processing sites\n");
	for (i = 0; i < sites.size(); i++) {
		struct map_site site;
		const Dictionary &site_data = sites[i];
		const Array &graphedges = site_data["graphedges"];
		printf("processing site: %d\n", i);
		site.graphedges.resize(graphedges.size());
		for (j = 0; j < graphedges.size(); j++) {
			const Dictionary &ge_data = graphedges[j];
			struct graphedge ge;
			ge.a = ge_data["a"];
			ge.b = ge_data["b"];
			ge.edge = ge_data["edge"];
			site.graphedges.write[j] = ge;
		}
	       	site.index = site_data["index"];
	       	site.pos = site_data["pos"];
		site.polygon = site_data["polygon"];
		site.vertices = site_data["vertices"];
		site.site_type = SITE_UNASSIGNED;
		site.cluster = -1;
		index_site(&site);
		hedge_count += site.hedges.size();
		map_sites.write[i] = site;
	}
	printf("processing sites done\n");
	/* Fill global half edges array and put in grid */
	printf("processing %d half edges\n", hedge_count);
	map_hedges.resize(hedge_count);
	hedge_idx = 0;
	for (i = 0; i < map_sites.size(); i++) {
		for (j = 0; j < map_sites[i].hedges.size(); j++) {
			/* bad bad constness */
			struct half_edge *hedge = &map_sites.write[i].hedges.write[j];
			map_hedges.write[hedge_idx] = hedge;
			add_hedge_to_grid(hedge);
			hedge_idx++;
		}
	}
	printf("processing half edges done\n");
	classify_sites();
	printf("processing done, sites count: %d\n", map_sites.size());
}

void RoadGrid::build(Ref<Curve> curve, Ref<OpenSimplexNoise> noise)
{
	rnd.instance();
	rnd->randomize();
	printf("build_diagram\n");
	// Dictionary diagram = build_diagram(8, 2 + (rnd->randi() % 2), 100, 100, 50);
	Dictionary diagram = build_diagram(8, 2, 100, 100, 500);
	printf("build_diagram done\n");
	printf("process_diagram\n");
	process_diagram(diagram);
	printf("process_diagram done\n");
	printf("%d %d\n", curve.is_valid(), noise.is_valid());
	assert(curve.is_valid() && noise.is_valid());
	int i, j;
	if (curve.is_valid() && noise.is_valid()) {
		printf("building 3rd dimention\n");
		diagram_vertex_heights.resize(diagram_vertices.size());
		for (i = 0; i < diagram_vertices.size(); i++) {
			float n = noise->get_noise_2dv(diagram_vertices[i]);
			float t = (n + 1.0f) * 0.5f;
			float d = MAX(1.0f, curve->interpolate_baked(t));
			d = CLAMP(d, 1.0f, 30.0f);
			diagram_vertex_heights.write[i] = d;
		}
		for (j = 0; j < 3; j++) {
			for (i = 0; i < map_hedges.size(); i++) {
				int x1 = map_hedges[i]->a;
				int x2 = map_hedges[i]->b;
				float xd = map_hedges[i]->length;
				float dh = fabsf(diagram_vertex_heights[x2] - diagram_vertex_heights[x1]);
				if (fabsf(dh / xd) > 0.01f)
					diagram_vertex_heights.write[x2] = diagram_vertex_heights[x1] + dh / fabsf(dh) * 0.01f * xd;
			}
#if 0
			for (i = 0; i < diagram_vertices.size(); i++)
				diagram_vertex_heights.write[i] = Math::stepify(diagram_vertex_heights.write[i], 4.0f);
			for (i = 0; i < diagram_vertices.size(); i++)
				diagram_vertex_heights.write[i] = 2.0;
#endif
		}
		printf("building 3rd dimention done\n");
	}
}

Vector2 RoadGrid::get_influence(int x, int y, float radius) const
{
	int rd = (int)(radius / grid_width) + 1;
	List<struct half_edge *> hlist;
	List<struct half_edge *>::Element *e;
	int i = 0, j = 0;
	for (i = -rd; i < rd + 1; i++)
		for (j = -rd; j < rd + 1; j++) {
			List<struct half_edge *> tmp;
			if (hedge_grid.has(x / grid_width + i) && hedge_grid[x / grid_width + i].has(y / grid_height + j)) {
				tmp = hedge_grid[x / grid_width + i][y / grid_height + j];
				for (e = tmp.front(); e; e = e->next()) {
					struct half_edge *d = e->get();
					hlist.push_back(d);
				}
			}

		}
	if (hlist.size() == 0)
		return Vector2();
	for (e = hlist.front(); e; e = e->next()) {
		struct half_edge *he = e->get();
		Vector2 a = diagram_vertices[he->a];
		Vector2 b = diagram_vertices[he->b];
		Vector2 p(x, y);
		Vector2 seg[] = {a, b};
		Vector2 pt = Geometry::get_closest_point_to_segment_2d(p, seg);
		float d = pt.distance_squared_to(p);
		if (d < radius * radius) {
			Vector2 ret;
			ret.x = 1.0f;
			assert(diagram_vertex_heights.size() > he->a);
			assert(diagram_vertex_heights.size() > he->b);
			float h1 = diagram_vertex_heights[he->a];
			float h2 = diagram_vertex_heights[he->b];
			float l = he->length;
			assert(l > 0.0f);
			float m1 = pt.distance_to(a) / l;
			float m2 = CLAMP(1.0f - m1, 0.0f, 1.0f);
			float h = h1 * (1.0f - m1) + h2 * (1.0f - m2);
			ret.y = h - 2.5f;
			return ret;
		}
	}
	return Vector2();
}

void RoadGrid::_bind_methods()
{
	ClassDB::bind_method(D_METHOD("draw_debug", "drawable", "size_x", "size_y"), &RoadGrid::draw_debug);
	ClassDB::bind_method(D_METHOD("get_influence", "x", "y", "radius"), &RoadGrid::get_influence);
	ClassDB::bind_method(D_METHOD("build", "curve", "noise"), &RoadGrid::build);
}