#include <cassert>
#include <core/resource.h>
#include <core/sort_array.h>
#include <scene/resources/packed_scene.h>
#include <scene/main/viewport.h>
#include <scene/3d/camera.h>
#include <scene/3d/physics_body.h>
#include <scene/3d/collision_shape.h>
#include "roads.h"

void Roads::_bind_methods()
{
	ClassDB::bind_method(D_METHOD("curve_mesh", "points", "width", "flags", "sidewalk_width"), &Roads::curve_mesh);
	ClassDB::bind_method(D_METHOD("add_scene_element", "root", "surface", "p2", "shape"), &Roads::add_scene_element);
}
void Roads::_get_property_list(List<PropertyInfo> *p_list) const
{
	p_list->push_back(PropertyInfo(Variant::OBJECT, "road_data", PROPERTY_HINT_RESOURCE_TYPE, "PackedScene"));
	p_list->push_back(PropertyInfo(Variant::OBJECT, "curve", PROPERTY_HINT_RESOURCE_TYPE, "Curve"));
	p_list->push_back(PropertyInfo(Variant::OBJECT, "noise", PROPERTY_HINT_RESOURCE_TYPE, "OpenSimplexNoise"));
}
bool Roads::_get(const StringName &p_name, Variant &r_ret) const
{
	if (p_name == "road_data") {
		r_ret = road_data;
		return true;
	} else if (p_name == "curve") {
		r_ret = curve;
		return true;
	} else if (p_name == "noise") {
		r_ret = noise;
		return true;
	}
	return false;
}
bool Roads::_set(const StringName &p_name, const Variant &p_value)
{
	bool update = false;
	if (p_name == "road_data") {
		road_data = p_value;
		update = true;
	} else if (p_name == "curve") {
		curve = p_value;
		update = true;
	} else if (p_name == "noise") {
		noise = p_value;
		update = true;
	}
	if (update) {
		update_all();
		_change_notify();
	}

	return update;
}
void Roads::sort_angle(Vector<int> &sort_data)
{
	struct comparator {
		Vector3 *vertices;
		bool operator()(int a, int b) const {
			Vector3 p1 = vertices[a];
			Vector3 p2 = vertices[b];
			Vector2 rp1(p1.x, p1.z);
			Vector2 rp2(p2.x, p2.z);
			return rp1.angle() < rp2.angle();
		}
	};
	SortArray<int, struct comparator> sorter;
	sorter.compare.vertices = vertices.write().ptr();
	sorter.sort(sort_data.ptrw(), sort_data.size());
}
int Roads::find_edge(int a, int b)
{
	int i;
	RoadGrid *rg = RoadsData::get_singleton()->get_road_grid();
	for (i = 0; i < rg->map_hedges.size(); i++) {
		if (rg->map_hedges[i]->a == a &&
				rg->map_hedges[i]->b == b)
			return i;
	}
	return -1;
}
void Roads::setup_vshapes()
{
	int i, j;
	RoadGrid *rg = RoadsData::get_singleton()->get_road_grid();
	vertices.resize(rg->diagram_vertices.size());
	for (i = 0; i < vertices.size(); i++) {
		vertices.write()[i].x = rg->diagram_vertices[i].x;
		vertices.write()[i].y = rg->diagram_vertex_heights[i];
		vertices.write()[i].z = rg->diagram_vertices[i].y;
	}
	List<struct vshape> vdata_list;
	for (i = 0; i < rg->map_hedges.size(); i++) {
		for (j = 0; j < rg->map_hedges.size(); j++) {
			if (i == j)
				continue;
			if (rg->map_hedges[i]->b !=
					rg->map_hedges[j]->a)
				continue;
			if (rg->map_hedges[i]->site !=
					rg->map_hedges[j]->site)
				continue;
			int a, b1, b2;
			struct vshape v;
			/* star topology */
			a = rg->map_hedges[i]->b;
			b1 = rg->map_hedges[i]->a;
			b2 = rg->map_hedges[j]->b;
			v.e1 = i;
			v.e2 = j;
			v.site = rg->map_hedges[i]->site;
			v.area.position = vertices[a];
			v.area.expand_to(vertices[b1] + Vector3(0, 1, 0));
			v.area.expand_to(vertices[b2] + Vector3(0, -1, 0));
			v.instance = -1;
			Vector3 p1 = vertices[rg->map_hedges[v.e1]->a];
			Vector3 p2 = vertices[rg->map_hedges[v.e1]->b];
			Vector3 p3 = vertices[rg->map_hedges[v.e2]->b];
			p1 = (p2 + (p1 - p2) * 0.5f).snapped(Vector3(4.0f, 0.1f, 4.0f));
			p3 = (p2 + (p3 - p2) * 0.5f).snapped(Vector3(4.0f, 0.1f, 4.0f));
			p2 = p2.snapped(Vector3(4.0f, 0.1f, 4.0f));
			v.p1 = p1;
			v.p2 = p2;
			v.p3 = p3;
			/* add v-shape only if we can actually generate it */
			if (v.p1.distance_squared_to(v.p2) > 2.0f &&
					v.p2.distance_squared_to(v.p3) > 2.0f &&
					v.p1.distance_squared_to(v.p3) > 2.0f)
				vdata_list.push_back(v);
		}
	}
	vshapes.resize(vdata_list.size());
	for (i = 0; i < vdata_list.size(); i++)
		vshapes.write()[i] = vdata_list[i];
	for (i = 0; i < vshapes.size(); i++) {
		for (j = 0; j < vshapes.size(); j++) {
			if (i == j)
				continue;
			if (vshapes[i].e1 == vshapes[j].e1)
				vshapes.write()[j].p1 = vshapes[i].p1;
			if (vshapes[i].e2 == vshapes[j].e1)
				vshapes.write()[j].p1 = vshapes[i].p3;
			if (vshapes[i].e1 == vshapes[j].e2)
				vshapes.write()[j].p3 = vshapes[i].p1;
			if (vshapes[i].e2 == vshapes[j].e2)
				vshapes.write()[j].p3 = vshapes[i].p3;
		}
	}

	for (i = 0; i < vshapes.size(); i++) {
		const struct vshape &v = vshapes[i];
		assert(rg->map_hedges[v.e1]->site == rg->map_hedges[v.e2]->site);
		assert(v.e1 >= 0 && v.e2 >= 0 && v.e1 != v.e2);
		int e1a = rg->map_hedges[vshapes[i].e1]->a;
		int e1b = rg->map_hedges[vshapes[i].e1]->b;
		int e2a = rg->map_hedges[vshapes[i].e2]->a;
		int e2b = rg->map_hedges[vshapes[i].e2]->b;
		printf("vshape %d: %d: %d: %f %f %f -> %d: %f %f %f -> %d: %d: %f %f %f -> %d: %f %f %f\n",
				i,
				vshapes[i].e1,
				e1a,
				vertices[e1a].x,
				vertices[e1a].y,
				vertices[e1a].z,
				e1b,
				vertices[e1b].x,
				vertices[e1b].y,
				vertices[e1b].z,
				vshapes[i].e2,
				e2a,
				vertices[e2a].x,
				vertices[e2a].y,
				vertices[e2a].z,
				e2b,
				vertices[e2b].x,
				vertices[e2b].y,
				vertices[e2b].z
		      );
	}
}
void Roads::update_all()
{
	int i;
	RoadGrid *rg = RoadsData::get_singleton()->get_road_grid();
	if (road_data.is_valid()) {
		Node *tmp = road_data->instance();
		for (i = 0; i < tmp->get_child_count(); i++) {
			Node *c = tmp->get_child(i);
			MeshInstance *mi = Object::cast_to<MeshInstance>(c);
			if (mi) {
				String name = mi->get_name();
				Ref<ArrayMesh> mesh = mi->get_mesh();
				Array data = mesh->surface_get_arrays(0);
				mesh_data[name] = data;
				if (name == "road_main")
					mat = mesh->surface_get_material(0);
			}
		}
		if (curve.is_valid() && noise.is_valid())
			rg->build(curve, noise);
		printf("vertices: %d\n", rg->diagram_vertices.size());
		printf("heights: %d\n", rg->diagram_vertex_heights.size());
		printf("edges: %d\n", rg->map_hedges.size());
		setup_vshapes();
	}
}

Roads::Roads()
{
	body = memnew(StaticBody);
}
Roads::~Roads()
{
	memdelete(body);
}
Vector3 Roads::calculate_offsets(const Array &data) const
{
	float minx = 0.0f, maxx = 0.0f, minz = 0.0f, maxz = 0.0f;
	PoolVector<Vector3> verts = data[Mesh::ARRAY_VERTEX];
	int i;
	Vector3 ret;
	for (i = 0; i < verts.size(); i++) {
		if (minx > verts.read()[i].x)
			minx = verts.read()[i].x;
		if (maxx < verts.read()[i].x)
			maxx = verts.read()[i].x;
		if (minz > verts.read()[i].z)
			minz = verts.read()[i].z;
		if (maxz < verts.read()[i].z)
			maxz = verts.read()[i].z;
	}
	ret.x = fabsf(maxx - minx);
	ret.z = fabsf(maxz - minz);
	return ret;
}
Vector3 Roads::quadratic_bezier(const Vector3 &p0, const Vector3 &p1,
		const Vector3 &p2, float t) const
{
	Vector3 q0 = p0.linear_interpolate(p1, t);
	Vector3 q1 = p1.linear_interpolate(p2, t);
	Vector3 r = q0.linear_interpolate(q1, t);
	return r;
}
void Roads::all_offsets()
{
	const String *k;
	for (k = mesh_data.next(NULL); k; k = mesh_data.next(k))
		offsets[*k] = calculate_offsets(mesh_data[*k]);
}
enum {
	FLAGS_SIDEWALK = (1 << 0),
	FLAGS_INTERSECTION = (1 << 1),
	FLAGS_WALL = (1 << 2),
};

PoolVector<String> Roads::build_item_list(float width, int flags, float sidewalk_width) const
{
	PoolVector<String> ret;
	float tx = 0.0f;
	while (tx < width) {
		ret.push_back("road_main");
		tx += offsets["road_main"].x;
	}
	if (flags & FLAGS_SIDEWALK) {
		ret.push_back("sidewalk_start");
		tx = 0.0f;
		while (tx < sidewalk_width) {
			ret.push_back("sidewalk");
			tx += offsets["sidewalk"].x;
		}
		ret.push_back("sidewalk_end");
	}
	if (flags & FLAGS_WALL)
		ret.push_back("wall");
	return ret;
}
Ref<Curve3D> Roads::build_curve(Vector3 p1, Vector3 p2, Vector3 p3, float total_width) const
{
	Ref<Curve3D> curve3;
	curve3.instance();
	Vector3 dir1 = (p1 - p2).normalized();
	Vector3 dir2 = (p3 - p2).normalized();
	float qt = 0.0f;
	curve3->add_point(p1);
	while (qt <= 1.0f) {
		Vector3 p = quadratic_bezier(p2 + dir1 * total_width * 1.8f, p2, p2 + dir2 * total_width * 1.8f, qt);
		curve3->add_point(p.snapped(Vector3(0.1, 0.1, 0.1)));
		qt += 0.1f;
	}
	curve3->add_point(p3);
	curve3->set_bake_interval(4.0f);
	assert(curve3->get_baked_length() > 0);
	return curve3;
}

Array Roads::curve_mesh(PoolVector<Vector3> points, float width, int flags, float sidewalk_sidth)
{
	float tx = 0.0f, total_width = 0.0f, t = 0.0f, l;
	int i;
	Array ret;
	all_offsets();
	PoolVector<String> parts_list = build_item_list(width, flags, sidewalk_sidth);
	for (i = 0; i < parts_list.size(); i++)
		total_width += offsets[parts_list[i]].x;
	assert(total_width >= 3.0f);
	Ref<Curve3D> curve3 = build_curve(points[0], points[1], points[2], total_width);
	l = curve3->get_baked_length();
	assert(l > 0.0f);
	PoolVector<Vector3> new_verts, new_normals;
	PoolVector<Vector2> new_uvs;
	PoolVector<int> new_index;
	ret.resize(Mesh::ARRAY_MAX);

	while (t <= l) {
		tx = 0.0f;
		int part = 0;
		while (tx < total_width) {
			int k;
			Array data = mesh_data[parts_list[part]];
			int b = new_verts.size();
			PoolVector<Vector3> verts = data[Mesh::ARRAY_VERTEX];
			PoolVector<Vector3> normals = data[Mesh::ARRAY_NORMAL];
			new_verts.resize(b + verts.size());
			new_normals.resize(b + normals.size());
			Transform xform;
			Vector3 offt1, offt2;
			for (k = 0; k < verts.size(); k++) {
				Vector3 base = verts[k];
				float point = t + 2.0 + base.z;
				float right = verts[k].x + tx;
				if (t <= l - 2.0) {
					offt1 = curve3->interpolate_baked(point, true);
					offt2 = curve3->interpolate_baked(point + 2.0, true);
					assert(offt1.distance_squared_to(offt2) > 0.0f);
					xform = Transform(Basis(), offt1).looking_at(offt2, Vector3(0.0f, 1.0f, 0.0f));
				} else {
					offt1 = curve3->interpolate_baked(point - 2.0, true);
					offt2 = curve3->interpolate_baked(point, true);
					assert(offt1.distance_squared_to(offt2) > 0.0f);
					xform = Transform(Basis(), offt1).looking_at(offt2, Vector3(0.0f, 1.0f, 0.0f));
					offt1 = offt2;
				}
				xform.origin = Vector3();
				if (right < 0.25f)
					right -= 0.15f;
				Vector3 nvert = offt1 + xform.xform(Vector3(right, verts[k].y, 0.0));
				Vector3 n = xform.xform(normals[k]);
				if (right < 0.15f &&
						((flags & FLAGS_INTERSECTION) != 0) &&
						nvert.distance_squared_to(points[1]) < total_width * total_width * 2.5f) {
					new_verts.write()[b + k] = points[1];
					new_normals.write()[b + k] = Vector3(0.0f, 1.0f, 0.0f);
				} else {
					new_verts.write()[b + k] = nvert;
					new_normals.write()[b + k] = n;
				}
			}
			new_uvs.append_array(data[Mesh::ARRAY_TEX_UV]);
			int idx = new_index.size();
			PoolVector<int> index = data[Mesh::ARRAY_INDEX];
			new_index.resize(idx + index.size());
			for (k = 0; k < index.size(); k++)
				new_index.write()[idx + k] = index[k] + b;
			tx += offsets[parts_list[part]].x;
			part += 1;
			if (part >= parts_list.size())
				break;
		}
		t += 2.0f;
	}
	ret[Mesh::ARRAY_VERTEX] = new_verts;
	ret[Mesh::ARRAY_NORMAL] = new_normals;
	ret[Mesh::ARRAY_TEX_UV] = new_uvs;
	ret[Mesh::ARRAY_INDEX] = new_index;

	return ret;
}

static Ref<ConcavePolygonShape> create_concave_polygon_shape(Vector<Array> surfaces) {
	PoolVector<Vector3> face_points;
	int face_points_size = 0;

	//find the correct size for face_points
	for (int i = 0; i < surfaces.size(); i++) {
		const Array &surface_arrays = surfaces[i];
		if (surface_arrays.size() == 0) {
			// That surface is empty
			continue;
		}
		// If the surface is not empty then it must have an expected amount of data arrays
		ERR_CONTINUE(surface_arrays.size() != Mesh::ARRAY_MAX);
		PoolVector<int> indices = surface_arrays[Mesh::ARRAY_INDEX];
		face_points_size += indices.size();
	}
	face_points.resize(face_points_size);

	if (face_points_size < 3) {
		return Ref<ConcavePolygonShape>();
	}

	//copy the points into it
	int face_points_offset = 0;
	for (int i = 0; i < surfaces.size(); i++) {
		const Array &surface_arrays = surfaces[i];
		if (surface_arrays.size() == 0) {
			continue;
		}
		PoolVector<Vector3> positions = surface_arrays[Mesh::ARRAY_VERTEX];
		PoolVector<int> indices = surface_arrays[Mesh::ARRAY_INDEX];

		ERR_FAIL_COND_V(positions.size() < 3, Ref<ConcavePolygonShape>());
		ERR_FAIL_COND_V(indices.size() < 3, Ref<ConcavePolygonShape>());
		ERR_FAIL_COND_V(indices.size() % 3 != 0, Ref<ConcavePolygonShape>());

		int face_points_count = face_points_offset + indices.size();

		{
			PoolVector<Vector3>::Write w = face_points.write();
			PoolVector<int>::Read index_r = indices.read();
			PoolVector<Vector3>::Read position_r = positions.read();

			for (int p = face_points_offset; p < face_points_count; ++p) {
				w[p] = position_r[index_r[p - face_points_offset]];
			}
		}

		face_points_offset += indices.size();
	}

	Ref<ConcavePolygonShape> shape = memnew(ConcavePolygonShape);
	shape->set_faces(face_points);
	return shape;
}

int Roads::make_vmesh(Node *root, Ref<Material> mat, Ref<ArrayMesh> mesh, MeshInstance *xmi, Vector3 p1, Vector3 p2,
			Vector3 p3, float width, int flags, float sidewalk_width)
{
	Vector3 m1 = p1 - p2;
	Vector3 m2 = Vector3();
	Vector3 m3 = p3 - p2;
	Vector3 pts[] = {m1, m2, m3};
	int i;
	PoolVector<Vector3> points;
	assert(p1.distance_squared_to(p2) > 2.0f);
	assert(p2.distance_squared_to(p3) > 2.0f);
	assert(p1.distance_squared_to(p3) > 2.0f);
	assert(m1.distance_squared_to(m2) > 2.0f);
	assert(m2.distance_squared_to(m3) > 2.0f);
	assert(m1.distance_squared_to(m3) > 2.0f);
	points.resize(3);
	for (i = 0; i < 3; i++)
		points.write()[i] = pts[i].snapped(Vector3(4.0f, 0.1f, 4.0f));
	Array rdata = curve_mesh(points, width, flags, sidewalk_width);
	Ref<ArrayMesh> mdata = mesh;
	assert(mdata.is_valid());
	mdata->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, rdata);
	assert(mdata->get_surface_count() > 0);
	Vector<Array> surfaces;
	surfaces.push_back(rdata);
	Ref<ConcavePolygonShape> shape = create_concave_polygon_shape(surfaces);
	mdata->surface_set_material(0, mat);
	xmi->set_mesh(mdata);
	call_deferred("add_scene_element", root, xmi, p2, shape);
	return xmi->get_instance_id();
}

void Roads::add_scene_element(Node *root, Node *xnode, const Vector3 &p2, Ref<ConcavePolygonShape> shape)
{
	MeshInstance *xmi = Object::cast_to<MeshInstance>(xnode);
	if (!xmi)
		return;
	root->add_child(xmi);
	Transform xform(Basis(), p2);
	assert(xmi->get_mesh().is_valid() && xmi->get_mesh()->get_surface_count() > 0);
	xmi->set_global_transform(xform);
	CollisionShape *cs = memnew(CollisionShape);
	cs->set_shape(shape);
	body->add_child(cs);
}

void Roads::process_vshapes()
{
	Transform xform = get_viewport()->get_camera()->get_global_transform();
	AABB camarea;
	camarea.position = xform.origin;
	camarea.grow_by(550.0f);
	int i;
	List<int> active_vshapes;
	printf("camera %f %f %f\n", xform.origin.x, xform.origin.y, xform.origin.z);
	for (i = 0; i < vshapes.size(); i++) {
		if (active_vshapes.size() > 32)
			break;
		if (vshapes[i].instance >= 0)
			continue;
#if 0
		active_vshapes.push_back(i);
#else
		if (vshapes[i].area.intersects(camarea))
			active_vshapes.push_back(i);
		else if (vshapes[i].area.encloses(camarea))
			active_vshapes.push_back(i);
		else if (camarea.encloses(vshapes[i].area))
			active_vshapes.push_back(i);
		else if (camarea.intersects(vshapes[i].area))
			active_vshapes.push_back(i);
#endif
	}
	printf("active vshapes %d\n", active_vshapes.size());
	List<int>::Element *e;
	for (e = active_vshapes.front(); e; e = e->next()) {
		i = e->get();
		const struct vshape &v = vshapes[i];
		assert(v.p1.distance_squared_to(v.p2) > 2.0f);
		assert(v.p2.distance_squared_to(v.p3) > 2.0f);
		assert(v.p1.distance_squared_to(v.p3) > 2.0f);
		if (vshapes[i].instance < 0) {
			if (thread.thread.is_started())
				thread.thread.wait_to_finish();
			thread.mat = mat;
			thread.vshape = i;
			thread.width = 6.0;
			thread.sidewalk_width = 3.0;
			thread.flags = FLAGS_SIDEWALK|FLAGS_INTERSECTION;
			thread.root = this;
			Ref<ArrayMesh> mesh;
			mesh.instance();
			thread.mesh = mesh;
			thread.xmi = memnew(MeshInstance);
			thread.thread.start(generate_threaded, &thread);
		}
	}
}

void Roads::_notification(int p_what)
{
	switch(p_what) {
	case NOTIFICATION_PROCESS:
		if ((counter % 100) == 0)
			process_vshapes();
		counter++;
		break;
	case NOTIFICATION_READY:
		counter = 0;
		set_process(true);
		add_child(body);
		break;
	}
}
void Roads::generate_threaded(void *p_userdata)
{
	struct thread_data *data = (struct thread_data *)p_userdata;
	Roads *obj = Object::cast_to<Roads>(data->root);
	obj->mutex.lock();
	Vector3 p1 = obj->vshapes[data->vshape].p1;
	Vector3 p2 = obj->vshapes[data->vshape].p2;
	Vector3 p3 = obj->vshapes[data->vshape].p3;
	obj->mutex.unlock();
	int instance = obj->make_vmesh(obj, data->mat, data->mesh, data->xmi, p1, p2, p3, data->width, data->flags, data->sidewalk_width);
	assert(instance >= 0);
	obj->mutex.lock();
	obj->vshapes.write()[data->vshape].instance = instance;
	obj->mutex.unlock();
}


RoadsData::RoadsData()
{
	rg = memnew(RoadGrid);
}
RoadsData::~RoadsData()
{
	memdelete(rg);
	rg = NULL;
}
static RoadsData *g_roads_data = NULL;
RoadsData* RoadsData::get_singleton()
{
	return g_roads_data;
}

void  RoadsData::create_singleton()
{
	g_roads_data = memnew(RoadsData);
}
void  RoadsData::destroy_singleton()
{
	memdelete(g_roads_data);
	g_roads_data = NULL;
}
RoadGrid *RoadsData::get_road_grid()
{
	return rg;
}
void RoadsData::_bind_methods()
{
	ClassDB::bind_method(D_METHOD("get_road_grid"), &RoadsData::get_road_grid);
}