#undef NDEBUG
#include <cassert>
#include <algorithm>
#include <vector>
#include <unordered_map>
#include <core/io/config_file.h>
#include <core/io/json.h>
#include <core/os/dir_access.h>
#include <core/os/time.h>
#include <core/math/geometry.h>
#include <scene/3d/immediate_geometry.h>
#include <scene/main/viewport.h>
#include "from_string.h"
#include "buildings_data.h"
#include "road_lines_data.h"

ImmediateGeometry *RoadLinesData::debug_im = nullptr;
static Ref<Material> debug_material;
RoadLinesData::RoadLinesData()
{
	int i;
	ConfigFile config;
	Error result = config.load("res://config/stream.conf");
	ERR_FAIL_COND_MSG(result != OK, "Failed to load config");
	road_lines_path = config.get_value("lines", "road_lines_path");
	String road_lines_path = config.get_value("lines", "road_lines_path");
	String road_lines_json =
		FileAccess::get_file_as_string(road_lines_path);
	Variant json_v;
	String es;
	int eline;
	Error status = JSON::parse(road_lines_json, json_v, es, eline);
	ERR_FAIL_COND_MSG(status != OK, "Can't parse json: " + es +
						" at line: " + itos(eline));

	Dictionary json = json_v;
	List<Variant> keys;
	json.get_key_list(&keys);
	List<Variant>::Element *e = keys.front();
	while (e) {
		String key = e->get();
		struct road_line rline;
		Dictionary entry = json.get(key, Dictionary());
		Array points = entry.get("points", Array());
		Array indices = entry.get("indices", Array());
		rline.metadata = entry.get("metadata", Dictionary());
		int lanes = entry.get("lanes", -1);
		int pattern = entry.get("pattern", -1);
		rline.pattern = pattern;
		rline.points.resize(points.size());
		rline.indices.resize(indices.size());
		for (i = 0; i < (int)points.size(); i++) {
			String point_s = points[i];
			rline.points[i] = from_string<Transform>(point_s);
		}
		for (i = 0; i < (int)indices.size(); i++) {
			int index = indices[i];
			rline.indices[i] = index;
		}
		// TODO: wtf is flags?
		rline.lanes = lanes;
		lines[key] = rline;
		e = e->next();
	}
}
RoadLinesData *RoadLinesData::singleton = nullptr;
RoadLinesData *RoadLinesData::get_singleton()
{
	if (!singleton)
		singleton = memnew(RoadLinesData);
	return singleton;
}
RoadLinesData::~RoadLinesData()
{
#if 0
	if (debug_im) {
		memdelete(debug_im);
		debug_im = nullptr;
	}
#endif
}
void RoadLinesData::cleanup()
{
	memdelete(singleton);
	singleton = nullptr;
}
String RoadLinesData::get_road_lines_path()
{
	return road_lines_path;
}
void RoadLinesData::get_road_lines_key_list(List<String> *keys)
{
	List<String> line_keys;
	lines.get_key_list(&line_keys);
	List<String>::Element *e = line_keys.front();
	keys->clear();
	while (e) {
		const String &key = e->get();
		if (key.ends_with("_road"))
			keys->push_back(key);
		e = e->next();
	}
}
void RoadLinesData::save_data()
{
	int i;
	ConfigFile config;
	Error result = config.load("res://config/stream.conf");
	ERR_FAIL_COND_MSG(result != OK, "Failed to load config");
	String road_lines_path = config.get_value("road", "road_lines_path");
	String road_lines_json =
		FileAccess::get_file_as_string(road_lines_path);
	Dictionary output;
	List<String> keys;
	lines.get_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		Dictionary pvalues;
		Array points, indices;
		points.resize(lines[e->get()].points.size());
		for (i = 0; i < (int)lines[e->get()].points.size(); i++)
			points[i] = to_string(lines[e->get()].points[i]);
		indices.resize(lines[e->get()].indices.size());
		for (i = 0; i < (int)lines[e->get()].indices.size(); i++)
			indices[i] = lines[e->get()].indices[i];
		pvalues["points"] = points;
		// pvalues["indices"] = indices;
		pvalues["metadata"] = lines[e->get()].metadata;
		pvalues["lanes"] = lines[e->get()].lanes;
		pvalues["pattern"] = lines[e->get()].pattern;
		output[e->get()] = pvalues;
		e = e->next();
	}
	print_verbose(JSON::print(output, "\t", false));
	Error err = OK;
	if (FileAccess::exists(road_lines_path)) {
		DirAccess *dir = DirAccess::open("res:///", &err);
		assert(dir && err == OK);
		err = dir->copy(
			road_lines_path,
			road_lines_path + "." +
				String::num(
					Time::get_singleton()
						->get_unix_time_from_system()));
	}
	FileAccess *fd =
		FileAccess::open(road_lines_path, FileAccess::WRITE, &err);
	if (err == OK) {
		fd->store_string(JSON::print(output, "\t", false));
		fd->close();
	}
}
uint32_t RoadLinesData::road_lines_hash(const Vector3 &v)
{
	int x = (int)(v.x / 100);
	int y = (int)(v.y / 100);
	int z = (int)(v.z / 100);
	return x ^ (y * 100) ^ (z * 10000);
}
void RoadLinesData::road_lines_curve_index(
	struct RoadLinesData::road_line &rline,
	std::unordered_map<uint32_t, std::vector<Vector3> >
		&road_lines_nodes_hash,
	std::vector<Vector3> &road_lines_nodes)
{
	int i, j;
	rline.indices.clear();
	for (i = 0; i < (int)rline.points.size(); i++) {
		Vector3 pt = rline.points[i].origin;
		int pt_hash = road_lines_hash(pt);
		if (road_lines_nodes_hash.find(pt_hash) !=
		    road_lines_nodes_hash.end()) {
			bool ok = true;
			for (j = 0;
			     j < (int)road_lines_nodes_hash[pt_hash].size();
			     j++) {
				const Vector3 &xpt =
					road_lines_nodes_hash[pt_hash][j];
				if (xpt.distance_squared_to(pt) < 160) {
					ok = false;
					pt = xpt;
					break;
				}
			}
			if (ok) {
				road_lines_nodes_hash[pt_hash].push_back(pt);
				road_lines_nodes.push_back(pt);
			}
		} else {
			road_lines_nodes.push_back(pt);
			road_lines_nodes_hash[pt_hash] = { pt };
		}
		std::vector<Vector3>::iterator it = std::find(
			road_lines_nodes.begin(), road_lines_nodes.end(), pt);
		assert(it != road_lines_nodes.end());
		int index = it - road_lines_nodes.begin();
		rline.indices.push_back(index);
	}
}
void RoadLinesData::index_lines(
	std::unordered_map<uint32_t, std::vector<Vector3> >
		&road_lines_nodes_hash,
	std::vector<Vector3> &road_lines_nodes)
{
	List<String> keys;
	get_road_lines_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		String rkey = e->get();
		struct RoadLinesData::road_line &pt = lines[rkey];
		pt.indices.clear();
		e = e->next();
	}
	e = keys.front();
	while (e) {
		String rkey = e->get();
		struct RoadLinesData::road_line &pt = lines[rkey];
		road_lines_curve_index(pt, road_lines_nodes_hash,
				       road_lines_nodes);
		e = e->next();
	}
}
static inline int get_segment_index(const String &road, int pos)
{
	RoadLinesData *rld = RoadLinesData::get_singleton();
	int idx = rld->lines[road].indices[pos];
	return idx;
}
void RoadLinesData::create_segments(const String &road,
				    std::vector<int> &segments)
{
	int i;
	for (i = 0; i < (int)lines[road].indices.size() - 1; i++) {
		segments.push_back(i);
		segments.push_back(i + 1);
	}
}
/* add close points on each line to the line */
void RoadLinesData::insert_close_points(std::vector<Vector3> &road_lines_nodes,
					float distance_squared)
{
	int i;
	List<String> keys;
	get_road_lines_key_list(&keys);
	List<String>::Element *e = keys.front();
	for (i = 0; i < (int)road_lines_nodes.size(); i++) {
		int idx3 = i;
		Vector3 p3 = road_lines_nodes[idx3];
		/* Checking each road point against
		   all line segments */
		while (e) {
			int j;
			std::vector<int> segments;
			String rkey = e->get();
			create_segments(rkey, segments);
			for (j = 0; j < (int)segments.size(); j += 2) {
				/* indices in road_lines_nodes */
				int idx1 = get_segment_index(rkey, segments[j]);
				int idx2 = get_segment_index(rkey,
							     segments[j + 1]);
				/* insertion point in line indices
				   array to split segment and add point */
				int idx = segments[j + 1];
				/* Skip segment point */
				if (idx3 == idx1 || idx3 == idx2)
					continue;
				Vector3 p1 = road_lines_nodes[idx1];
				Vector3 p2 = road_lines_nodes[idx2];
				std::vector<Vector3> seg = { p1, p2 };
				Vector3 closest =
					Geometry::get_closest_point_to_segment(
						p3, seg.data());
				/* should be no duplicate points
				   in road_lines_nodes */
				if (closest.is_equal_approx(p1))
					continue;
				if (closest.is_equal_approx(p2))
					continue;
				if (p3.distance_squared_to(closest) <
				    distance_squared) {
					/* split segment and replace road
					   point with a point on segment */
					lines[rkey].indices.insert(
						lines[rkey].indices.begin() +
							idx,
						idx3);
					road_lines_nodes[idx3] = closest;
				}
			}
			e = e->next();
		}
	}
}
void RoadLinesData::update_road_lines_nodes(
	std::vector<Vector3> &road_lines_nodes)
{
	List<String> keys;

	get_road_lines_key_list(&keys);
	std::unordered_map<uint32_t, std::tuple<String, String> > kcmp;
	{
		List<String>::Element *k = keys.front();
		List<String>::Element *r = keys.front();
		while (k) {
			String kkey = k->get();
			uint32_t kkey_hash = kkey.hash();
			while (r) {
				String rkey = r->get();
				uint32_t rkey_hash = rkey.hash();
				uint32_t key = kkey_hash ^ rkey_hash;
				uint32_t key2 = rkey_hash ^ kkey_hash;
				if (kcmp.find(key) == kcmp.end() &&
				    kcmp.find(key2) == kcmp.end())
					kcmp[key] = std::make_tuple(k->get(),
								    r->get());
				r = r->next();
			}
			k = k->next();
		}
	}
	using checks_tuple =
		std::tuple<String, int, int, int, String, int, int, int>;
	std::unordered_map<uint32_t, checks_tuple> checks;
	std::unordered_map<uint32_t, checks_tuple>::iterator checks_it;
	std::unordered_map<uint32_t, std::tuple<String, String> >::iterator it;
	for (it = kcmp.begin(); it != kcmp.end(); it++) {
		int i, j;
		std::tuple<String, String> data = kcmp[it->first];
		const String &k = std::get<0>(data);
		const String &r = std::get<1>(data);
		if (lines[k].indices.size() < 2)
			continue;
		if (lines[r].indices.size() < 2)
			continue;
		for (i = 0; i < (int)lines[k].indices.size() - 1; i++) {
			for (j = 0; j < (int)lines[k].indices.size() - 1; j++) {
				uint32_t key = k.hash() ^ i ^ r.hash() ^ j ^
					       2147483137;
				uint32_t key2 = r.hash() ^ j ^ k.hash() ^ i ^
						2147463167;
				if (checks.find(key) == checks.end() &&
				    checks.find(key2) == checks.end()) {
					int idx_a1 = lines[k].indices[i];
					int idx_a2 = lines[k].indices[i + 1];
					int idx_b1 = lines[k].indices[j];
					int idx_b2 = lines[k].indices[j + 1];
					std::vector<int> cmp1 = { idx_a1,
								  idx_a2 };
					if (std::find(cmp1.begin(), cmp1.end(),
						      idx_b1) != cmp1.end())
						continue;
					if (std::find(cmp1.begin(), cmp1.end(),
						      idx_b2) != cmp1.end())
						continue;
					checks[key] = std::make_tuple(
						k, i, idx_a1, idx_a2, r, j,
						idx_b1, idx_b2);
				}
			}
		}
	}
	/*
	for ch in checks.values():
		var k = ch[0]
		var i1 = ch[1]
		var idx_a1 = ch[2]
		var idx_a2 = ch[3]
		var r = ch[4]
		var j1 = ch[5]
		var idx_b1 = ch[6]
		var idx_b2 = ch[7]
		var p_a1 = road_lines_nodes[idx_a1]
		var p_a2 = road_lines_nodes[idx_a2]
		var p_b1 = road_lines_nodes[idx_b1]
		var p_b2 = road_lines_nodes[idx_b2]
		var px = Geometry.get_closest_points_between_segments(p_a1, p_a2, p_b1, p_b2)
		var d = px[0].distance_squared_to(px[1])
		if d < 160:
			var pxt = px[0].linear_interpolate(px[1], 0.5)
			var nidx = road_lines_nodes.size()
			road_lines_nodes.push_back(pxt)
			var il = road_lines[k].indices.size()
			assert(!nidx in road_lines[k].indices)
			assert(!nidx in road_lines[r].indices)
			road_lines[k].indices.insert(i1 + 1, nidx)
			road_lines[r].indices.insert(j1 + 1, nidx)
##end
		*/
	for (checks_it = checks.begin(); checks_it != checks.end();
	     checks_it++) {
		String k = std::get<0>(checks_it->second);
		int i = std::get<1>(checks_it->second);
		int idx_a1 = std::get<2>(checks_it->second);
		int idx_a2 = std::get<3>(checks_it->second);
		String r = std::get<4>(checks_it->second);
		int j = std::get<5>(checks_it->second);
		int idx_b1 = std::get<6>(checks_it->second);
		int idx_b2 = std::get<7>(checks_it->second);
		Vector3 p_a1 = road_lines_nodes[idx_a1];
		Vector3 p_a2 = road_lines_nodes[idx_a2];
		Vector3 p_b1 = road_lines_nodes[idx_b1];
		Vector3 p_b2 = road_lines_nodes[idx_b2];
		Vector3 px, px2;
		Geometry::get_closest_points_between_segments(p_a1, p_a2, p_b1,
							      p_b2, px, px2);
		float d = px.distance_squared_to(px2);
		if (d < 160) {
			Vector3 pxt = px.linear_interpolate(px2, 0.5f);
			int nidx = road_lines_nodes.size();
			road_lines_nodes.push_back(pxt);
			//				int il = (int)road_lines[k].indices.size();
			assert(std::find(lines[k].indices.begin(),
					 lines[k].indices.end(),
					 nidx) == lines[k].indices.end());
			assert(std::find(lines[r].indices.begin(),
					 lines[r].indices.end(),
					 nidx) == lines[r].indices.end());
			lines[k].indices.insert(
				lines[k].indices.begin() + i + 1, nidx);
			lines[r].indices.insert(
				lines[k].indices.begin() + j + 1, nidx);
		}
	}
}
void RoadLinesData::dump_road_lines(const std::vector<Vector3> &road_lines_nodes)
{
	int i;
	List<String> keys;
	get_road_lines_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		String rkey = e->get();
		struct RoadLinesData::road_line &pt = lines[rkey];
		String outline = rkey + ": ";
		for (i = 0; i < (int)pt.indices.size(); i++) {
			outline += " " + itos(pt.indices[i]);
		}
		for (i = 0; i < (int)pt.indices.size(); i++) {
			outline += " " + (road_lines_nodes[pt.indices[i]]
						  .operator String());
		}
		e = e->next();
	}
}
ImmediateGeometry *RoadLinesData::get_debug_node()
{
	if (!debug_im) {
		debug_im = memnew(ImmediateGeometry);
		Ref<SpatialMaterial> tmpmat;
		tmpmat.instance();
		tmpmat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR,
				 true);
		tmpmat->set_flag(SpatialMaterial::FLAG_DISABLE_DEPTH_TEST,
				 true);
		tmpmat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
		debug_material = tmpmat;
		debug_im->set_material_override(debug_material);
		SceneTree::get_singleton()
			->get_current_scene()
			->get_viewport()
			->call_deferred("add_child", debug_im);
	}
	return debug_im;
}
void RoadLinesData::process_lines(
	std::unordered_map<uint32_t, std::vector<Vector3> >
		&road_lines_nodes_hash,
	std::vector<Vector3> &road_lines_nodes)
{
	index_lines(road_lines_nodes_hash, road_lines_nodes);
	insert_close_points(road_lines_nodes, 160.0f);
	update_road_lines_nodes(road_lines_nodes);
	dump_road_lines(road_lines_nodes);
}
void RoadLinesData::set_debug_flags(int debug_flags)
{
	this->debug_flags = debug_flags;
}
int RoadLinesData::get_debug_flags() const
{
	return debug_flags;
}

void RoadLinesData::update_line_segments(const String &line)
{
	int i;
	lines[line].segments.clear();
	lines[line].segments.resize(lines[line].points.size() - 1);
	float offset = 0.0f;
	for (i = 0; i < (int)lines[line].points.size() - 1; i++) {
		struct line_segment segment;
		segment.p1 = lines[line].points[i].origin;
		segment.p2 = lines[line].points[i + 1].origin;
		segment.length = segment.p1.distance_to(segment.p2);
		segment.dir = (segment.p2 - segment.p1).normalized();
		Vector3 side = segment.dir.cross(Vector3(0, 1, 0));
		side.y = 0;
		segment.tangent = side.normalized();
		segment.offset = offset;
		lines[line].segments[i] = segment;
		offset += segment.length;
	}
}

void RoadLinesData::line_add_building(const String &line, const String &key,
				      float curve_offset, float normal_offset)
{
	int index = BuildingsData::get_singleton()->get_building_by_key(key);
	if (index < 0)
		return;
	struct line_building_data lb;
	lb.building_key = key;
	lb.building_key_hash = key.hash64();
	lb.line_offset = curve_offset;
	lb.normal_offset = normal_offset;
	lines[line].buildings.push_back(lb);
	// TODO: save/load
	BuildingsData::get_singleton()->buildings[index].line_name = line;
}

void RoadLinesData::assign_close_buildings(const String &line)
{
	int i, j;
	print_line("assign_close_buildings: " + line);
	if (!lines.has(line))
		return;
	if (!line.ends_with("_buildings"))
		return;
	if (lines[line].points.size() < 2)
		return;
	update_line_segments(line);
	if (lines[line].segments.size() == 0)
		return;
	print_line("assign_close_buildings: processing: " +
		   itos(BuildingsData::get_singleton()->buildings.size()) +
		   " buildings");
	for (i = 0; i < (int)BuildingsData::get_singleton()->buildings.size();
	     i++) {
		float dst = Math_INF;
		float result_offset = 0.0f;
		float side = 0.0f;
		String building_key;
		struct BuildingsData::building &data =
			BuildingsData::get_singleton()->buildings[i];
		// manually placed
		if (!data.generated)
			continue;
		// already assigned to another line
		const String &building_line =
			BuildingsData::get_singleton()->buildings[i].line_name;
		if (building_line != line && building_line != "")
			continue;
		if (line_has_building(line, data.key))
			continue;
		Vector3 p = data.xform.origin;
		Vector3 segment_point;
		int segment_index = -1;

		for (j = 0; j < (int)lines[line].segments.size(); j++) {
			Vector3 seg[] = { lines[line].segments[j].p1,
					  lines[line].segments[j].p2 };
			Vector3 closest =
				Geometry::get_closest_point_to_segment(p, seg);
			Vector3 xp = p;
			xp.y = p.y;
			float tmpdst = xp.distance_squared_to(closest);
			if (closest.is_equal_approx(
				    lines[line].segments[j].p1) ||
			    closest.is_equal_approx(
				    lines[line].segments[j].p2)) {
				float w = closest.dot(
					lines[line].segments[j].dir);
				float wt = xp.dot(lines[line].segments[j].dir);
				if (wt - w > 0.1f || wt - w < -0.1f)
					continue;
			}
			if (dst > tmpdst) {
				dst = tmpdst;
				building_key = data.key;
				segment_index = j;
				segment_point = closest;
			}
		}
		if (segment_index < 0)
			continue;

		const Vector3 &p1 = lines[line].segments[segment_index].p1;
		const Vector3 &p2 = lines[line].segments[segment_index].p2;
		const Vector3 &dir = lines[line].segments[segment_index].dir;
		assert(segment_index >= 0);
		// assert(!segment_point.is_equal_approx(p1));
		// assert(!segment_point.is_equal_approx(p2));
		assert((segment_point - p1).dot(dir) >= 0);
		float wdst = (p - p1).dot(dir);
		print_line("wdst=" + String::num(wdst));
		assert(wdst >= 0);
		result_offset =
			lines[line].segments[segment_index].offset + wdst;
		side = (p - p1).dot(lines[line].segments[j].tangent);

		assert(result_offset >= 0);

		print_line("key: " + building_key +
			   " dst: " + String::num(dst));
		if (dst < 16 * 16) {
			print_line("adding: key: " + building_key +
				   " dst: " + String::num(dst));
			assert(result_offset >= 0);
			line_add_building(line, building_key, result_offset,
					  side);
		}
	}
}

bool RoadLinesData::line_has_building(const String &line,
				      const String &building_key)
{
	uint64_t key_hash = building_key.hash64();
	bool ret = false;
	int i;
	for (i = 0; i < (int)lines[line].buildings.size(); i++)
		if (lines[line].buildings[i].building_key_hash == key_hash) {
			ret = true;
			break;
		}
	return ret;
}

Vector3 RoadLinesData::get_point_by_offsets(const String &line,
					    float dir_offset,
					    float normal_offset)
{
	Vector3 ret;
	int i;
	print_verbose("line: " + line +
		      " line_offset: " + String::num(dir_offset) +
		      " normal_offset: " + String::num(normal_offset));
	float n_offset = dir_offset;
	int selected_segment = 0;
	for (i = 0; i < (int)lines[line].segments.size(); i++) {
		struct line_segment *segment = &lines[line].segments[i];
		if (n_offset < segment->length) {
			selected_segment = i;
			break;
		}
		n_offset -= segment->length;
	}
	ret = lines[line].segments[selected_segment].p1 +
	      lines[line].segments[selected_segment].dir * n_offset +
	      lines[line].segments[selected_segment].tangent * normal_offset;
	return ret;
}