#undef NDEBUG
#include <cassert>
#include <algorithm>
#include <vector>
#include <unordered_map>
#include <core/reference.h>
#include <core/io/config_file.h>
#include <core/os/file_access.h>
#include <scene/resources/mesh.h>
#include <scene/3d/mesh_instance.h>
#include <scene/3d/immediate_geometry.h>
#include <core/math/transform.h>
#include <core/math/geometry.h>
#include <core/math/vector2.h>
#include <core/math/vector3.h>
#include <core/hash_map.h>
#include "from_string.h"
#include "road_debug.h"
#include "road_lines_data.h"
#include "buildings_data.h"
#include "road_processing.h"

struct wedge {
	Vector3 p[3];
	Vector3 y[3];
	int width1, width2;
};

struct RoadLinesProcessing {
	std::vector<Vector3> nodes;
	struct edgedata {
		std::vector<int> neighbors;
	};
	std::unordered_map<int, struct edgedata> edges;
	std::unordered_map<uint32_t, std::vector<struct wedge> > wedges;
	String road_center_mesh_path, road_mid_mesh_path,
		road_sidewalk_mesh_path;
	int debug_flags;
	static struct RoadLinesProcessing *singleton;
	static RoadLinesProcessing *get_singleton()
	{
		if (!singleton)
			singleton = memnew(RoadLinesProcessing);
		return singleton;
	}
	RoadLinesProcessing()
	{
		singleton = this;
	}

	void create_nodes(const std::vector<Vector3> &road_lines_nodes)
	{
		nodes.resize(road_lines_nodes.size());
		memcpy(nodes.data(), road_lines_nodes.data(),
		       sizeof(Vector3) * road_lines_nodes.size());
	}
	Transform get_structure_transform(Vector3 p0, Vector3 p1, Vector3 dir,
					  Vector3 offset_n, float offset,
					  float dir_offset, float y_rotation,
					  float y_offset)
	{
		Transform xform(Basis(), p0 + dir * dir_offset +
						 offset_n * offset +
						 Vector3(0, 1, 0) * y_offset);
		Transform rot(Basis().rotated(Vector3(0, 1, 0),
					      Math::deg2rad(y_rotation)),
			      Vector3());
		xform.set_look_at(xform.origin, xform.origin + dir * dir_offset,
				  Vector3(0, 1, 0));
		xform = xform * rot;
		return xform;
	}
	void create_building_structure(const BuildingsData::building &b)
	{
		assert(b.key.begins_with("road__"));
		assert(b.id.length() > 0);
		assert(b.key.length() > 0);
		if (BuildingsData::get_singleton()->has_building(b.key)) {
			BuildingsData::get_singleton()->remove_scene_item(
				b.id, b.key);
			BuildingsData::get_singleton()->destroy_building(b.key);
		}
		BuildingsData::get_singleton()->create_building(b);
		if (!BuildingsData::get_singleton()->has_scene(b.id))
			BuildingsData::get_singleton()->create_scene_data(
				b.id, b.key);
		else
			BuildingsData::get_singleton()->add_scene_item(b.id,
								       b.key);
		print_line("created building: " + b.key);
		print_line("at: " + (b.xform.origin.operator String()));
	}
	int create_sideroads(const String &line_key, int edge_no,
			     const RoadLinesData::road_edge &edge,
			     const Vector3 &p0, const Vector3 &p1,
			     const Vector3 &dir, const Vector3 left_n,
			     const Vector3 &right_n)
	{
		int structures_generated = 0;
		if (edge.left.sideroad > 0) {
			Transform xform = get_structure_transform(
				p0, p1, dir, left_n, edge.left.sideroad_offset,
				edge.left.sideroad_dir_offset,
				edge.left.sideroad_y_rotation,
				edge.left.sideroad_y_offset);
			BuildingsData::building b;
			b.key = "road__" + line_key + "__sideroad__" +
				itos(edge_no) + "__left__sideroad-" +
				edge.left.sideroad_type;
			b.id = "sideroad-" + edge.left.sideroad_type;
			b.generated = true;
			b.line_name = line_key;
			b.key_hash = b.key.hash64();
			b.pattern_id = 0;
			b.xform = xform;
			b.worktime[0] = 0;
			b.worktime[1] = 23;
			if (edge.left.sideroad_type.length() == 0) {
				print_error("bad sideroad");
				goto out;
			}
			create_building_structure(b);
			structures_generated++;
out:;
		}
		if (edge.right.sideroad > 0) {
			Transform xform = get_structure_transform(
				p0, p1, dir, right_n,
				edge.right.sideroad_offset,
				edge.right.sideroad_dir_offset,
				edge.right.sideroad_y_rotation,
				edge.right.sideroad_y_offset);
			BuildingsData::building b;
			b.key = "road__" + line_key + "__sideroad__" +
				itos(edge_no) + "__right__sideroad-" +
				edge.right.sideroad_type;
			b.id = "sideroad-" + edge.right.sideroad_type;
			b.generated = true;
			b.line_name = line_key;
			b.key_hash = b.key.hash64();
			b.pattern_id = 0;
			b.xform = xform;
			b.worktime[0] = 0;
			b.worktime[1] = 23;
			if (edge.right.sideroad_type.length() == 0) {
				print_error("bad sideroad");
				goto out2;
			}
			create_building_structure(b);
			structures_generated++;
out2:;
		}
		return structures_generated;
	}
	int
	create_line_building(const String &line_key, int edge_id,
			     const Transform &xform,
			     const struct RoadLinesData::road_edge_side &side,
			     const String &side_name)
	{
		int building_no;
		int structures_generated = 0;
		for (building_no = 0; building_no < (int)side.buildings.size();
		     building_no++) {
			const struct RoadLinesData::road_edge_side::buildings
				&building = side.buildings[building_no];
			String id = side.buildings[building_no].id;
			id = id.strip_edges();
			if (id.length() == 0)
				continue;
			Transform rot(
				Basis().rotated(
					Vector3(0, 1, 0),
					Math::deg2rad(building.y_rotation)),
				building.offsets);
			Transform building_xform = xform * rot;
			BuildingsData::building bbuilding;
			bbuilding.id = id;
			bbuilding.key = "road__" + line_key + "__lot__" +
					itos(edge_id) + "__" + side_name +
					"__" + itos(building_no) +
					"__building-" + building.id;
			bbuilding.generated = true;
			bbuilding.line_name = line_key;
			bbuilding.key_hash = bbuilding.key.hash64();
			bbuilding.pattern_id = 0;
			bbuilding.xform = building_xform;
			bbuilding.worktime[0] = 0;
			bbuilding.worktime[1] = 23;
			create_building_structure(bbuilding);
			structures_generated++;
		}
		return structures_generated;
	}
	int create_lots(const String &line_key, int edge_no,
			const RoadLinesData::road_edge &edge, const Vector3 &p0,
			const Vector3 &p1, const Vector3 &dir,
			const Vector3 left_n, const Vector3 &right_n)
	{
		int structures_generated = 0;
		if (edge.left.lot > 0) {
			Transform xform = get_structure_transform(
				p0, p1, dir, left_n, edge.left.lot_offset,
				edge.left.lot_dir_offset,
				edge.left.lot_y_rotation,
				edge.left.lot_y_offset);
			BuildingsData::building b;
			b.key = "road__" + line_key + "__lot__" +
				itos(edge_no) + "__left__lot-" +
				edge.left.lot_type;
			b.id = "lot-" + edge.left.lot_type;
			b.generated = true;
			b.line_name = line_key;
			b.key_hash = b.key.hash64();
			b.pattern_id = 0;
			b.xform = xform;
			b.worktime[0] = 0;
			b.worktime[1] = 23;
			create_building_structure(b);
			structures_generated++;
			if (edge.left.buildings.size() > 0) {
				structures_generated += create_line_building(
					line_key, edge_no, xform, edge.left,
					"left");
			}
		}
		if (edge.right.lot > 0) {
			Transform xform = get_structure_transform(
				p0, p1, dir, right_n, edge.right.lot_offset,
				edge.right.lot_dir_offset,
				edge.right.lot_y_rotation,
				edge.right.lot_y_offset);
			BuildingsData::building b;
			b.key = "road__" + line_key + "__lot__" +
				itos(edge_no) + "__right__lot-" +
				edge.right.lot_type;
			b.id = "lot-" + edge.right.lot_type;
			b.generated = true;
			b.line_name = line_key;
			b.key_hash = b.key.hash64();
			b.pattern_id = 0;
			b.xform = xform;
			b.worktime[0] = 0;
			b.worktime[1] = 23;
			create_building_structure(b);
			structures_generated++;
			if (edge.right.buildings.size() > 0) {
				structures_generated += create_line_building(
					line_key, edge_no, xform, edge.right,
					"right");
			}
		}
		return structures_generated;
	}
	int create_transit_stops(const String &line_key, int edge_no,
				 const RoadLinesData::road_edge &edge,
				 const Vector3 &p0, const Vector3 &p1,
				 const Vector3 &dir, const Vector3 left_n,
				 const Vector3 &right_n)
	{
		int j;
		int structures_generated = 0;
		float l = p0.distance_to(p1);
		if (edge.left.transit_stop_count > 0) {
			print_line("Creating transit stops on the left");
			float t = 0.0f;
			float m =
				l / ((float)(edge.left.transit_stop_count + 1));
			for (j = 0; j < edge.left.transit_stop_count; j++) {
				Transform xform = get_structure_transform(
					p0, p1, dir, left_n,
					edge.left.transit_stop_offset,
					t + edge.left.transit_stop_dir_offset,
					edge.left.transit_stop_y_rotation,
					0.0f);
				BuildingsData::building b;
				b.key = "road__" + line_key +
					"__transit_stop__" + itos(edge_no) +
					"__" + itos(j) + "__left__" +
					edge.left.transit_stop_type;
				b.id = edge.left.transit_stop_type;
				b.generated = true;
				b.line_name = line_key;
				b.key_hash = b.key.hash64();
				b.pattern_id = 0;
				b.xform = xform;
				b.worktime[0] = 0;
				b.worktime[1] = 23;
				create_building_structure(b);
				t += m;
				structures_generated++;
			}
		}
		if (edge.right.transit_stop_count > 0) {
			print_line("Creating transit stops on the right");
			float t = 0.0f;
			float m = l /
				  ((float)(edge.right.transit_stop_count + 1));
			for (j = 0; j < edge.right.transit_stop_count; j++) {
				Transform xform = get_structure_transform(
					p0, p1, dir, right_n,
					edge.right.transit_stop_offset,
					t + edge.right.transit_stop_dir_offset,
					edge.right.transit_stop_y_rotation,
					0.0f);
				BuildingsData::building b;
				b.key = "road__" + line_key +
					"__transit_stop__" + itos(edge_no) +
					"__" + itos(j) + "__right__" +
					edge.right.transit_stop_type;
				b.id = edge.right.transit_stop_type;
				b.generated = true;
				b.line_name = line_key;
				b.key_hash = b.key.hash64();
				b.pattern_id = 0;
				b.xform = xform;
				b.worktime[0] = 0;
				b.worktime[1] = 23;
				create_building_structure(b);
				t += m;
				structures_generated++;
			}
		}
		return structures_generated;
	}
	void create_structures()
	{
		List<String> keys;
		RoadLinesData *rld = RoadLinesData::get_singleton();
		rld->get_road_lines_key_list(&keys);
		List<String>::Element *e = keys.front();
		print_line("create structures");
		print_line("road_lines: " + itos(keys.size()));
		int structures_generated = 0;
		while (e) {
			const String &key = e->get();
			int i;
			if (rld->lines(key).points.size() < 2) {
				e = e->next();
				continue;
			}
			if (rld->lines(key).edges.size() !=
			    rld->lines(key).points.size() - 1) {
				e = e->next();
				continue;
			}
			print_line("line: " + key + " edges count: " +
				   itos((int)rld->lines(key).edges.size()));
			for (i = 0; i < (int)rld->lines(key).edges.size();
			     i++) {
				const RoadLinesData::road_edge &edge =
					rld->lines(key).edges[i];
				const Vector3 &p0 =
					rld->lines(key).points[i].origin;
				const Vector3 &p1 =
					rld->lines(key).points[i + 1].origin;
				Vector3 dir = (p1 - p0).normalized();
				Vector3 d = dir;
				Vector3 left_n = Vector3(0, 1, 0).cross(d);
				Vector3 right_n = -left_n;
				/* lots */
				structures_generated +=
					create_lots(key, i, edge, p0, p1, dir,
						    left_n, right_n);
				/* sideroads */
				structures_generated +=
					create_sideroads(key, i, edge, p0, p1,
							 dir, left_n, right_n);
				/* bus stops */
				structures_generated += create_transit_stops(
					key, i, edge, p0, p1, dir, left_n,
					right_n);
			}
			e = e->next();
		}
		print_line("structures generated: " +
			   itos(structures_generated));
	}
	void create_edges()
	{
		int i;
		List<String> keys;
		RoadLinesData *rld = RoadLinesData::get_singleton();
		edges.clear();
		rld->get_road_lines_key_list(&keys);
		List<String>::Element *e = keys.front();
		while (e) {
			const String &key = e->get();
			if (rld->lines(key).indices.size() < 2) {
				e = e->next();
				continue;
			}
			for (i = 0; i < (int)rld->lines(key).indices.size() - 1;
			     i++) {
				int idx1 = rld->lines(key).indices[i];
				int idx2 = rld->lines(key).indices[i + 1];
				if (edges.find(idx1) == edges.end()) {
					struct edgedata ed;
					ed.neighbors.clear();
					edges[idx1] = ed;
				}
				if (edges.find(idx2) == edges.end()) {
					struct edgedata ed;
					ed.neighbors.clear();
					edges[idx2] = ed;
				}
				if (std::find(edges[idx1].neighbors.begin(),
					      edges[idx1].neighbors.end(),
					      idx2) ==
				    edges[idx1].neighbors.end())
					edges[idx1].neighbors.push_back(idx2);
				if (std::find(edges[idx2].neighbors.begin(),
					      edges[idx2].neighbors.end(),
					      idx1) ==
				    edges[idx2].neighbors.end())
					edges[idx2].neighbors.push_back(idx1);
			}
			e = e->next();
		}
	}
	void sort_neighbors()
	{
		int i;
		/* sort neighbors by angle */
		for (i = 0; i < (int)nodes.size(); i++) {
			std::sort(
				edges[i].neighbors.begin(),
				edges[i].neighbors.end(),
				[i, this](int a, int b) {
					Vector3 na = nodes[a];
					Vector3 nb = nodes[b];
					Vector3 root = nodes[i];
					Vector3 xa = na - root;
					Vector3 xb = nb - root;
					float a1 = Vector2(xa.x, xa.z).angle();
					float a2 = Vector2(xb.x, xb.z).angle();
					return a1 < a2;
				});
		}
	}
	/* need to find nodes too close to edge and split them */
	void optimize_nodes(float maxdst)
	{
		int i, j, k;
		for (k = 0; k < (int)nodes.size(); k++) {
			const Vector3 &p = nodes[k];
			Vector3 np;
			float dst = Math_INF;
			int n1, n2;
			for (i = 0; i < (int)nodes.size(); i++) {
				if (k == i)
					continue;
				for (j = 0; j < (int)edges[i].neighbors.size();
				     j++) {
					const Vector3 &p1 = nodes[i];
					const Vector3 &p2 =
						nodes[edges[i].neighbors[j]];
					const Vector3 seg[] = { p1, p2 };
					Vector3 tmp = Geometry::
						get_closest_point_to_segment(
							p, &seg[0]);
					if (tmp.is_equal_approx(p1) ||
					    tmp.is_equal_approx(p2))
						/* edges of segment, bad */
						continue;
					float tmpdst =
						p.distance_squared_to(tmp);
					if (tmpdst > maxdst * maxdst)
						continue;
					if (dst > tmpdst) {
						dst = tmpdst;
						np = tmp;
						n1 = i;
						n2 = edges[i].neighbors[j];
					}
				}
			}
			if (dst <= maxdst * maxdst) {
				nodes[k] = np;
				edges[n2].neighbors.erase(std::remove(
					edges[n2].neighbors.begin(),
					edges[n2].neighbors.end(), n1));
				edges[n1].neighbors.erase(std::remove(
					edges[n1].neighbors.begin(),
					edges[n1].neighbors.end(), n2));
				if (std::find(edges[k].neighbors.begin(),
					      edges[k].neighbors.end(),
					      n1) == edges[k].neighbors.end())
					edges[k].neighbors.push_back(n1);
				if (std::find(edges[k].neighbors.begin(),
					      edges[k].neighbors.end(),
					      n2) == edges[k].neighbors.end())
					edges[k].neighbors.push_back(n2);
				if (std::find(edges[n1].neighbors.begin(),
					      edges[n1].neighbors.end(),
					      k) == edges[n1].neighbors.end())
					edges[n1].neighbors.push_back(k);
				if (std::find(edges[n2].neighbors.begin(),
					      edges[n2].neighbors.end(),
					      k) == edges[n2].neighbors.end())
					edges[n2].neighbors.push_back(k);
				print_verbose("FIXED: " + itos(k) + ": " +
					      String::num(dst));
			}
		}
	}
	Vector3 tangent(const Vector3 &v)
	{
		Vector2 rv = Vector2(v.x, v.z).tangent();
		return Vector3(rv.x, v.y, rv.y);
	}
	Vector3 normal(const Vector3 &v)
	{
		Vector3 rv = tangent(v);
		rv.y = 0.0f;
		return rv.normalized();
	}

	void build_wedges(
		std::unordered_map<uint32_t, std::vector<struct wedge> > &wedges)
	{
		int i;
		float road_side_width = 3.0f;
		std::vector<Vector3> neighbors;
		for (i = 0; i < (int)nodes.size(); i++) {
			const Vector3 &node = nodes[i];
			Vector3 fkey = node;
			fkey.y = 0.0f;
			uint32_t key = (int)fkey.x ^ ((int)fkey.z * 10);
			if (wedges.find(key) == wedges.end())
				wedges[key] = {};
			int j;
			neighbors.resize(edges[i].neighbors.size());
			for (j = 0; j < (int)edges[i].neighbors.size(); j++)
				neighbors[j] = nodes[edges[i].neighbors[j]];
			for (j = 0; j < (int)edges[i].neighbors.size(); j++) {
				if (edges[i].neighbors.size() == 0)
					continue;
				int onext = (j + 1) % edges[i].neighbors.size();
				Vector3 n1 = normal(node - neighbors[j]);
				Vector3 n2 = normal(neighbors[onext] - node);
				float angle = n1.signed_angle_to(
					n2, Vector3(0, 1, 0));
				Vector3 a0 = (neighbors[j] - node) * 0.5 +
					     node + n1 * 3.0f;
				Vector3 a1 = node + n1 * road_side_width;
				Vector3 b0 = node + n2 * road_side_width;
				Vector3 b1 = (neighbors[onext] - node) * 0.5 +
					     node + n2 * 3.0f;
				Vector3 q, r, pr;
				if (angle < 0 || angle > Math_PI) {
					Vector3 da = (a1 - a0).normalized() *
						     road_side_width * 2.0f;
					Vector3 db = (b1 - b0).normalized() *
						     road_side_width * 2.0f;
					Geometry::get_closest_points_between_segments(
						a0 - da, a1 + da, b0 - db,
						b1 + db, q, r);
					if (Vector2(q.x, q.z)
						    .distance_squared_to(
							    Vector2(r.x, r.z)) <
					    0.1f)
						pr = q.linear_interpolate(r,
									  0.5f);
					else {
						print_line(
							"bad luck: " +
							(q.operator String()) +
							" -> " +
							(r.operator String()));
						float e =
							Vector2(q.x, q.z).distance_squared_to(
								Vector2(r.x,
									r.z));
						print_line("dst " +
							   String::num(e));
						assert(false);
					}
					/* pr = node +
						     n1 * road_side_width; */
				} else {
					Geometry::get_closest_points_between_segments(
						a0, a1, b0, b1, q, r);
					if (Vector2(q.x, q.z)
						    .distance_squared_to(
							    Vector2(r.x, r.z)) <
					    0.001f)
						pr = q.linear_interpolate(r,
									  0.5f);
					else
						pr = node +
						     n1 * road_side_width;
				}
				Vector3 o1 = (neighbors[j] - node) * 0.5 + node;
				Vector3 o2 = node,
					o3 = (neighbors[onext] - node) * 0.5 +
					     node;
				struct wedge w;
				w.p[0] = a0;
				w.p[1] = pr;
				w.p[2] = b1;
				w.y[0] = o1;
				w.y[1] = o2;
				w.y[2] = o3;
				w.width1 = 2.0f * 5.0f;
				w.width2 = 2.0f * 5.0f;
				wedges[i].push_back(w);
			}
		}
	}
	~RoadLinesProcessing()
	{
	}
	static void cleanup()
	{
		if (singleton) {
			memfree(singleton);
			singleton = nullptr;
		}
	}
	void set_debug_flags(int debug_flags)
	{
		this->debug_flags = debug_flags;
	}
	int get_debug_flags() const
	{
		return debug_flags;
	}
	void road_setup()
	{
		int i, j;
		RoadLinesData *rld = RoadLinesData::get_singleton();
		std::vector<Vector3> road_lines_nodes;
		std::unordered_map<uint32_t, std::vector<Vector3> >
			road_lines_nodes_hash;
		road_lines_nodes.clear();
		road_lines_nodes_hash.clear();
		rld->set_debug_flags(debug_flags);
		rld->process_lines(road_lines_nodes_hash, road_lines_nodes);
		create_nodes(road_lines_nodes);
		create_edges();
		optimize_nodes(16.0f);
		sort_neighbors();
		wedges.clear();
		build_wedges(wedges);
		ImmediateGeometry *d = rld->get_debug_node();
		d->clear();
		if (debug_flags & 1) {
			d->begin(Mesh::PRIMITIVE_LINES);
			for (i = 0; i < (int)nodes.size(); i++) {
				d->set_color(Color(0.1f, 0.6f, 0.6f, 1.0f));
				d->add_vertex(nodes[i]);
				d->set_color(Color(0.1f, 0.6f, 0.6f, 1.0f));
				d->add_vertex(nodes[i] +
					      Vector3(0.0f, 200.0f, 0.0f));
			}
			d->end();
		}
		if (debug_flags & 2) {
			d->begin(Mesh::PRIMITIVE_LINES);
			for (i = 0; i < (int)nodes.size(); i++) {
				const struct edgedata &e = edges[i];
				for (j = 0; j < (int)e.neighbors.size(); j++) {
					int idx1 = i;
					int idx2 = e.neighbors[j];
					assert(idx1 != idx2);
					const Vector3 &p1 =
						nodes[idx1] + Vector3(0, 10, 0);
					const Vector3 &p2 =
						nodes[idx2] + Vector3(0, 10, 0);
					d->set_color(
						Color(0.1f, 0.6f, 0.6f, 1.0f));
					d->add_vertex(p1);
					d->set_color(
						Color(0.1f, 0.6f, 0.6f, 1.0f));
					d->add_vertex(p2);
				}
			}
			d->end();
		}
		if (debug_flags & 4) {
			d->begin(Mesh::PRIMITIVE_LINES);
			for (i = 0; i < (int)nodes.size(); i++) {
				std::vector<struct wedge> wedge = wedges[i];
				for (j = 0; j < (int)wedge.size(); j++) {
					struct wedge w = wedge[j];
					Vector3 p0 = w.p[0] + Vector3(0, 20, 0);
					Vector3 p1 = w.p[1] + Vector3(0, 30, 0);
					Vector3 p2 = w.p[2] + Vector3(0, 40, 0);
					d->set_color(
						Color(0.1f, 0.6f, 0.1f, 1.0f));
					d->add_vertex(p0);
					d->set_color(
						Color(0.1f, 0.6f, 0.1f, 1.0f));
					d->add_vertex(p1);
					d->set_color(
						Color(0.1f, 0.6f, 0.1f, 1.0f));
					d->add_vertex(p1);
					d->set_color(
						Color(0.1f, 0.6f, 0.1f, 1.0f));
					d->add_vertex(p2);
				}
			}
			d->end();
		}
		print_line("ROAD SETUP DONE");
	}
};
RoadLinesProcessing *RoadLinesProcessing::singleton;

class RoadMeshProcessing {
	struct mesh_data {
		Vector<Ref<Material> > materials;
		Vector<Array> arrays;
		Ref<ArrayMesh> create_mesh()
		{
			Ref<ArrayMesh> mesh;
			mesh.instance();
			int i;
			for (i = 0; i < arrays.size(); i++) {
				mesh->add_surface_from_arrays(
					Mesh::PRIMITIVE_TRIANGLES, arrays[i]);
				mesh->surface_set_material(i, materials[i]);
				return mesh;
			}
		}
	};
	HashMap<String, struct mesh_data> road_meshes;
	std::vector<MeshInstance *> nodes_mi;

public:
	void load_road_mesh(const String &category, const String &name,
			    const String &path)
	{
		Error err;
		int i;
		Ref<ArrayMesh> mesh =
			ResourceLoader::load(path, "ArrayMesh", true, &err);
		ERR_FAIL_COND_MSG(err != OK, "Failed to load: " + path);
		struct mesh_data md;
		int count = mesh->get_surface_count();
		md.materials.resize(count);
		md.arrays.resize(count);
		for (i = 0; i < count; i++) {
			Ref<Material> mat = mesh->surface_get_material(i);
			Array surfaces = mesh->surface_get_arrays(i);
			md.materials.write[i] = mat;
			md.arrays.write[i] = surfaces.duplicate();
		}
		road_meshes[category + "/" + name] = md;
	}
	struct wedge_paths {
		Vector3 path1[3];
		Vector3 path2[3];
	};
	void get_paths(struct wedge_paths &paths, const struct wedge &w)
	{
		Vector3 p1 = w.y[0] - w.y[1];
		Vector3 p2 = Vector3();
		Vector3 p3 = w.y[2] - w.y[1];
		Vector3 p1a = w.p[0] - w.y[1];
		Vector3 p2a = w.p[1] - w.y[1];
		Vector3 p3a = w.p[2] - w.y[1];
		paths.path1[0] = p1;
		paths.path1[1] = p2;
		paths.path1[2] = p3;
		paths.path2[0] = p1a;
		paths.path2[1] = p2a;
		paths.path2[2] = p3a;
	}
	struct lane {
		Vector3 p[3];
		String use_mesh;
		Transform xform1, xform2;
		Transform xform_m1, xform_m2;
		float l_seg1[3];
		float l_seg2[3];
	};
	void build_segment(const Transform &xform1, const Transform xform2,
			   float l0, float l1, const Array &arrays,
			   Array &out_arrays)
	{
		PoolVector<Vector3> vertices =
			arrays[ArrayMesh::ARRAY_VERTEX].duplicate();
		PoolVector<Vector3> normals =
			arrays[ArrayMesh::ARRAY_NORMAL].duplicate();
		PoolVector<float> tangents =
			arrays[ArrayMesh::ARRAY_TANGENT].duplicate();
		PoolVector<Vector2> uvs =
			arrays[ArrayMesh::ARRAY_TEX_UV].duplicate();
		PoolVector<int> indices =
			arrays[ArrayMesh::ARRAY_INDEX].duplicate();
		float dlen = xform1.origin.distance_to(xform2.origin);
		int id;
		for (id = 0; id < vertices.size(); id++) {
			Vector3 p = vertices[id];
			Vector3 n = normals[id];
			Vector2 uv = uvs[id];
			if (p.z < -0.3f) {
				//				p.z = -dlen;
				/* second segment */
				p.z = 0.0f;
				p.x *= l1;
				uv.y *= dlen;
				p = xform2.xform(p);
				n = Transform(xform2.basis, Vector3()).xform(n);
			} else {
				uv.y *= dlen;
				p.z = 0;
				//				p.z = 0;
				p.x *= l0;
				p = xform1.xform(p);
				n = Transform(xform1.basis, Vector3()).xform(n);
			}
			vertices.write()[id] = p;
			normals.write()[id] = n;
			uvs.write()[id] = uv;
		}
#if 0
		for (i = 0; i < arrays.size(); i++)
			out_arrays[i] = arrays[i];
		out_arrays[ArrayMesh::ARRAY_VERTEX] = vertices;
		/* normals are bad */
		out_arrays[ArrayMesh::ARRAY_NORMAL] = normals;
		out_arrays[ArrayMesh::ARRAY_TEX_UV] = uvs;
		return;
#endif
		PoolVector<int> out_index;
		out_index.resize(indices.size());
		assert(out_arrays.size() >= ArrayMesh::ARRAY_MAX);
		PoolVector<float> tg = out_arrays[ArrayMesh::ARRAY_TANGENT];
		tg.append_array(tangents);
		out_arrays[ArrayMesh::ARRAY_TANGENT] = tg;

		PoolVector<Vector3> out_vertices_orig =
			out_arrays[ArrayMesh::ARRAY_VERTEX];
		int index_offset = out_vertices_orig.size();
		out_vertices_orig.append_array(vertices);
		out_arrays[ArrayMesh::ARRAY_VERTEX] = out_vertices_orig;

		PoolVector<Vector3> out_normals_orig =
			out_arrays[ArrayMesh::ARRAY_NORMAL];
		out_normals_orig.append_array(normals);
		out_arrays[ArrayMesh::ARRAY_NORMAL] = out_normals_orig;

		PoolVector<Vector2> out_uvs_orig =
			out_arrays[ArrayMesh::ARRAY_TEX_UV];
		out_uvs_orig.append_array(uvs);
		out_arrays[ArrayMesh::ARRAY_TEX_UV] = out_uvs_orig;
		for (id = 0; id < indices.size(); id++)
			out_index.write()[id] = indices[id] + index_offset;

		PoolVector<int> out_index_orig =
			out_arrays[ArrayMesh::ARRAY_INDEX];
		out_index_orig.append_array(out_index);
		out_arrays[ArrayMesh::ARRAY_INDEX] = out_index_orig;
		// out_arrays[ArrayMesh::ARRAY_INDEX] = indices;
	}

	void init_surface(Array &surface)
	{
		surface.resize(ArrayMesh::ARRAY_MAX);
		surface[ArrayMesh::ARRAY_VERTEX] = PoolVector<Vector3>();
		surface[ArrayMesh::ARRAY_NORMAL] = PoolVector<Vector3>();
		surface[ArrayMesh::ARRAY_TANGENT] = PoolVector<float>();
		surface[ArrayMesh::ARRAY_TEX_UV] = PoolVector<Vector2>();
		surface[ArrayMesh::ARRAY_INDEX] = PoolVector<int>();
	}
	struct lane_params {
		Vector3 xp1, xp2, xp3;
		Vector3 xp1a, xp2a, xp3a;
		Vector3 edge1, edge2, edge3;
		Vector3 dir1, dir2;
		float l1, l2, l3;
		int nlanes, nlanes1, nlanes2;
		String center, mid, edge;
		lane_params(const struct wedge &wedge,
			    struct wedge_paths &paths, const String &center,
			    const String &mid, const String &edge)
		{
			nlanes1 = wedge.width1 / 5.0f;
			nlanes2 = wedge.width2 / 5.0f;
			nlanes = MAX(nlanes1, nlanes2);
			assert(nlanes < 16);
			xp1 = paths.path1[0];
			xp2 = paths.path1[1];
			xp3 = paths.path1[2];
			xp1a = paths.path2[0];
			xp2a = paths.path2[1];
			xp3a = paths.path2[2];
			dir1 = xp2 - xp1;
			dir2 = xp3 - xp2;
			edge1 = (xp1a - xp1).normalized();
			edge2 = (xp2a - xp2).normalized();
			edge3 = (xp3a - xp3).normalized();
			l1 = (xp1a - xp1).length();
			l2 = (xp2a - xp2).length();
			l3 = (xp3a - xp3).length();
			if (dir1.normalized().is_equal_approx(
				    dir2.normalized()))
				l2 = 3.0;
			this->center = center;
			this->mid = mid;
			this->edge = edge;
			if (!(l2 >= l1 - 0.0001f && l2 >= l3 - 0.0001f))
				print_line("bad parameters: l1 = " +
					   String::num(l1) +
					   " l2 = " + String::num(l2) +
					   " l3 = " + String::num(l3));
			assert(l2 >= l1 - 0.0001f && l2 >= l3 - 0.0001f);
			assert(l1 - 3.0f < 0.001f);
			assert(l3 - 3.0f < 0.001f);
			assert(center.length() > 0 && mid.length() > 0 &&
			       edge.length() > 0);
		}
	};
	void setup_lane(struct lane &lane, int index,
			const struct lane_params &params)
	{
		Vector3 p1 =
			params.xp1 + params.edge1 * float(index) * (params.l1);
		Vector3 p2 =
			params.xp2 + params.edge2 * float(index) * (params.l2);
		Vector3 p3 =
			params.xp3 + params.edge3 * float(index) * (params.l3);
		if (index >= params.nlanes) {
			p1 = params.xp1 +
			     params.edge1 * float(params.nlanes1) * (params.l1);
			p2 = params.xp2 +
			     params.edge2 * float(index + 0) * (params.l2);
			p3 = params.xp3 +
			     params.edge3 * float(params.nlanes2) * (params.l3);
		}
		lane.p[0] = p1;
		lane.p[1] = p2;
		lane.p[2] = p3;
		String use_mesh = params.center;
		if (index >= params.nlanes)
			use_mesh = params.edge;
		else if (params.nlanes > 1 && index == 0)
			use_mesh = params.center;
		else if (params.nlanes > 1 && index > 0)
			use_mesh = params.mid;
		lane.use_mesh = use_mesh;
		assert(use_mesh.length() > 0);
		assert(lane.use_mesh.length() > 0);
#if 0
		lane.xform1 = Transform(Basis(), lane.p[0]);
		lane.xform_m1 = Transform(Basis(), lane.p[1]);
		lane.xform_m2 = Transform(Basis(), lane.p[1]);
		lane.xform2 = Transform(Basis(), lane.p[2]);
#endif
		Transform xform1 =
			Transform().looking_at(params.dir1, Vector3(0, 1, 0));
		xform1.origin = lane.p[0];
		lane.xform1 = xform1;
		Transform xform2 =
			Transform().looking_at(params.dir2, Vector3(0, 1, 0));
		xform2.origin = lane.p[2];
		lane.xform2 = xform2;
		Vector3 dir_m1 =
			params.edge2.rotated(Vector3(0, 1, 0), Math_PI / 2.0);
		Transform xform_m1 =
			Transform().looking_at(-dir_m1, Vector3(0, 1, 0));
		xform_m1.origin = lane.p[1];
		lane.xform_m1 = xform_m1;
		Transform xform_m2 =
			Transform().looking_at(-dir_m1, Vector3(0, 1, 0));
		xform_m2.origin = lane.p[1];
		lane.xform_m2 = xform_m2;
		int i;
		float l_s[] = { params.l1, params.l2, params.l3 };
		for (i = 0;
		     i < (int)(sizeof(lane.l_seg1) / sizeof(lane.l_seg1[0]));
		     i++) {
			lane.l_seg1[i] = l_s[i];
			lane.l_seg2[i] = l_s[i];
		}
		if (index > params.nlanes1 - 1 && index < params.nlanes1)
			lane.l_seg1[0] = 0;
		if (index > params.nlanes2 - 1 && index < params.nlanes2)
			lane.l_seg2[2] = 0;
	}
	void build_wedge_mesh(const struct wedge &wedge, const String &center,
			      const String &mid, const String &edge,
			      std::vector<Array> &out_surfaces,
			      std::vector<Ref<Material> > &out_materials)
	{
		struct wedge_paths paths;
		get_paths(paths, wedge);
		int k;
		int segment_count = 0;
		std::vector<struct lane> lanes;
		bool sidewalk = true;
		struct lane_params params(wedge, paths, center, mid, edge);
		int parts = params.nlanes;
		if (sidewalk)
			parts = params.nlanes + 1;
		lanes.resize(parts);
		for (k = 0; k < (int)lanes.size(); k++) {
			setup_lane(lanes[k], k, params);
			assert(lanes[k].use_mesh.length() > 0);
		}
		for (k = 0; k < (int)lanes.size(); k++) {
			std::vector<Array> surfaces;
			std::vector<Ref<Material> > materials;
			int h;
			surfaces.resize(
				road_meshes[lanes[k].use_mesh].arrays.size());
			materials.resize(
				road_meshes[lanes[k].use_mesh].arrays.size());
			assert(surfaces.size() > 0);
			assert(materials.size() > 0);
			for (h = 0;
			     h < road_meshes[lanes[k].use_mesh].arrays.size();
			     h++) {
				surfaces[h] =
					road_meshes[lanes[k].use_mesh].arrays[h];
				materials[h] = road_meshes[lanes[k].use_mesh]
						       .materials[h];
			}
			/* assuming the same surface count for all meshes */
			if (out_surfaces.size() > 0) {
				for (h = 0; h < road_meshes[lanes[k].use_mesh]
							.arrays.size();
				     h++) {
					if (out_surfaces[h].size() == 0) {
						Array surface;
						init_surface(surface);
						out_surfaces[h] = surface;
						out_materials[h] = materials[h];
					}
				}
			} else if (out_surfaces.size() == 0) {
				for (h = 0; h < road_meshes[lanes[k].use_mesh]
							.arrays.size();
				     h++) {
					Array surface;
					init_surface(surface);
					out_surfaces[h] = surface;
					out_materials[h] = materials[h];
				}
				assert(road_meshes[lanes[k].use_mesh]
					       .arrays.size() > 0);
			}
			assert(out_surfaces.size() > 0);
			for (h = 0;
			     h < road_meshes[lanes[k].use_mesh].arrays.size();
			     h++) {
				assert(out_surfaces[h].size() >=
				       ArrayMesh::ARRAY_MAX);
				build_segment(lanes[k].xform1,
					      lanes[k].xform_m1,
					      lanes[k].l_seg1[0],
					      lanes[k].l_seg1[1], surfaces[h],
					      out_surfaces[h]);
				build_segment(lanes[k].xform_m2,
					      lanes[k].xform2,
					      lanes[k].l_seg2[1],
					      lanes[k].l_seg2[2], surfaces[h],
					      out_surfaces[h]);
			}
			segment_count++;
		}
	}
	Ref<ArrayMesh> build_road(const std::vector<struct wedge> &wedges,
				  const String &center, const String &mid,
				  const String &edge)
	{
		int i;
		std::vector<Array> out_surfaces;
		std::vector<Ref<Material> > out_materials;
		int surf_count = road_meshes[center].arrays.size();
		out_surfaces.resize(surf_count);
		out_materials.resize(surf_count);
		//		Transform mesh_xform =
		//			Transform().rotated(Vector3(0, 1, 0), Math_PI);
		for (i = 0; i < (int)wedges.size(); i++) {
			build_wedge_mesh(wedges[i], center, mid, edge,
					 out_surfaces, out_materials);
		}
		Ref<ArrayMesh> new_mesh;
		new_mesh.instance();
		for (i = 0; i < (int)out_surfaces.size(); i++) {
			if (out_surfaces[i].size() > 0) {
				new_mesh->add_surface_from_arrays(
					Mesh::PRIMITIVE_TRIANGLES,
					out_surfaces[i]);
				new_mesh->surface_set_material(
					i, out_materials[i]);
			}
		}
		new_mesh->surface_set_name(0, "main");
		return new_mesh;
	}
	void clear_road_meshes()
	{
		int i;
		for (i = 0; i < (int)nodes_mi.size(); i++)
			nodes_mi[i]->queue_delete();
		nodes_mi.clear();
	}
	void create_road_meshes(Node *base)
	{
		int i;
		RoadLinesProcessing *r = RoadLinesProcessing::get_singleton();
		clear_road_meshes();
		for (i = 0; i < (int)r->nodes.size(); i++) {
			Ref<ArrayMesh> mesh =
				build_road(r->wedges[i], "common/center",
					   "common/mid", "common/sidewalk");
			MeshInstance *mi = memnew(MeshInstance);
			mi->hide();
			mi->set_mesh(mesh);
			Transform xform(Basis(), r->nodes[i]);
			base->call_deferred("add_child", mi);
			mi->set_transform(xform);
			mi->call_deferred("show");
			nodes_mi.push_back(mi);
		}
	}
	static RoadMeshProcessing *singleton;
	static RoadMeshProcessing *get_singleton()
	{
		if (!singleton)
			singleton = memnew(RoadMeshProcessing);
		return singleton;
	}
	RoadMeshProcessing()
	{
		singleton = this;
	}
	static void cleanup()
	{
		if (singleton) {
			memdelete(singleton);
			singleton = nullptr;
		}
	}
};
RoadMeshProcessing *RoadMeshProcessing::singleton;

void RoadProcessing::road_setup(Node *target, int debug_flags)
{
	RoadLinesProcessing::get_singleton()->create_structures();
	RoadLinesProcessing::get_singleton()->set_debug_flags(debug_flags);
	RoadLinesProcessing::get_singleton()->road_setup();
	RoadMeshProcessing::get_singleton()->create_road_meshes(target);
}

void RoadProcessing::remove_road_meshes(Node *target)
{
	RoadMeshProcessing::get_singleton()->clear_road_meshes();
}

void RoadProcessing::load_data()
{
	/* Not needed but still */
	RoadLinesData::get_singleton();
	ConfigFile config;
	Error result = config.load("res://config/stream.conf");
	ERR_FAIL_COND_MSG(result != OK, "Failed to load config");
	RoadMeshProcessing::get_singleton()->load_road_mesh(
		"common", "center", config.get_value("road", "center_mesh"));
	RoadMeshProcessing::get_singleton()->load_road_mesh(
		"common", "mid", config.get_value("road", "mid_mesh"));
	RoadMeshProcessing::get_singleton()->load_road_mesh(
		"common", "sidewalk",
		config.get_value("road", "sidewalk_mesh"));
}

void RoadDebug::_notification(int which)
{
	int i, j;
	RoadLinesProcessing *r = RoadLinesProcessing::get_singleton();
	std::unordered_map<int, RoadLinesProcessing::edgedata>::iterator it;
	switch (which) {
	case NOTIFICATION_ENTER_TREE:
		set_process(true);
		break;
	case NOTIFICATION_PROCESS:
		if (r->nodes.size() > 0 && r->edges.size() > 0) {
			VisualServer::get_singleton()->immediate_clear(imm);
			VisualServer::get_singleton()->immediate_begin(
				imm, VisualServer::PRIMITIVE_LINES, RID());
			VisualServer::get_singleton()->immediate_color(
				imm, Color(1.0f, 0.6f, 0.6f, 1.0f));
			for (it = r->edges.begin(); it != r->edges.end();
			     it++) {
				int idx1 = it->first;
				if (it == r->edges.begin()) {
					aabb.position = r->nodes[idx1];
					aabb.size = Vector3();
				} else
					aabb.expand_to(r->nodes[idx1]);
				struct RoadLinesProcessing::edgedata data =
					it->second;
				for (i = 0; i < (int)data.neighbors.size();
				     i++) {
					int idx2 = data.neighbors[i];
					aabb.expand_to(r->nodes[idx2]);
					Vector3 d = (r->nodes[idx2] -
						     r->nodes[idx1])
							    .normalized() *
						    0.5f;
					VisualServer::get_singleton()
						->immediate_vertex(
							imm,
							r->nodes[idx1] + d);
					VisualServer::get_singleton()
						->immediate_vertex(
							imm,
							r->nodes[idx2] - d);
				}
			}
			VisualServer::get_singleton()->immediate_color(
				imm, Color(0.6f, 0.6f, 1.0f, 1.0f));
			for (it = r->edges.begin(); it != r->edges.end();
			     it++) {
				int idx1 = it->first;
				VisualServer::get_singleton()->immediate_vertex(
					imm,
					r->nodes[idx1] - Vector3(0, 10, 0));
				VisualServer::get_singleton()->immediate_vertex(
					imm,
					r->nodes[idx1] + Vector3(0, 100, 0));
			}
			VisualServer::get_singleton()->immediate_color(
				imm, Color(0.6f, 1.0f, 0.6f, 1.0f));
			for (i = 0; i < (int)r->nodes.size(); i++) {
				VisualServer::get_singleton()->immediate_vertex(
					imm, r->nodes[i] - Vector3(0, 5, 0));
				VisualServer::get_singleton()->immediate_vertex(
					imm, r->nodes[i] + Vector3(0, 80, 0));
			}
			VisualServer::get_singleton()->immediate_color(
				imm, Color(1.0f, 1.0f, 0.6f, 1.0f));
			Vector3 l(0, 1.0, 0);
			for (i = 0; i < (int)r->nodes.size(); i++) {
				for (j = 0; j < (int)r->wedges[i].size(); j++) {
					VisualServer::get_singleton()
						->immediate_vertex(
							imm,
							r->wedges[i][j].p[0] +
								l);
					VisualServer::get_singleton()
						->immediate_vertex(
							imm,
							r->wedges[i][j].p[1] +
								l);
					VisualServer::get_singleton()
						->immediate_vertex(
							imm,
							r->wedges[i][j].p[1] +
								l);
					VisualServer::get_singleton()
						->immediate_vertex(
							imm,
							r->wedges[i][j].p[2] +
								l);
				}
			}
			VisualServer::get_singleton()->immediate_end(imm);
			set_process(false);
		}
		break;
	case NOTIFICATION_EXIT_TREE:
		break;
	}
}
void RoadProcessing::cleanup()
{
	RoadLinesProcessing::cleanup();
	RoadMeshProcessing::cleanup();
}