#include <cassert>
#include <core/resource.h>
#include <core/os/os.h>
#include <scene/main/scene_tree.h>
#include <scene/main/viewport.h>
#include <scene/3d/camera.h>
#include <scene/resources/packed_scene.h>
#include <scene/3d/vehicle_body.h>

#include "traffic.h"

Traffic::Traffic(): max_spawn_distance(120.0f),
	min_spawn_distance(80),
	delete_distance(Vector3(100.0f, -60.0f, 180.0f)),
	physics_distance(Vector3(30.0f, -20.0f, 60.0f)),
	list_cooldown(0.0), debug(false)
{
	state = 0;
	rnd.instance();
	rnd->randomize();
	call_deferred("idle_runner");
}

Traffic::~Traffic()
{
}

void Traffic::_bind_methods()
{
	ClassDB::bind_method(D_METHOD("idle_runner"), &Traffic::idle_runner);
	ClassDB::bind_method(D_METHOD("add_traffic_vehicle", "scene"), &Traffic::add_traffic_vehicle);
	ClassDB::bind_method(D_METHOD("remove_traffic_vehicle", "scene"), &Traffic::remove_traffic_vehicle);
}

void Traffic::add_traffic_vehicle(Ref<PackedScene> scene)
{
	scenes.push_back(scene);
	printf("traffic: added_scene: %d\n", scenes.size());
}
void Traffic::remove_traffic_vehicle(Ref<PackedScene> scene)
{
	scenes.erase(scene);
	printf("traffic: removed_scene: %d\n", scenes.size());
}
void Traffic::idle_runner()
{
	uint64_t before, after;
	float delta;

	SceneTree *tree = SceneTree::get_singleton();
	Viewport *view;
	Camera *camera;
	Transform cam_xform;
	before = OS::get_singleton()->get_ticks_usec();
	assert(tree);
	delta = tree->get_idle_process_time();
	if (state > 0 && debug)
		debug_traffic();

	if (state > 0)
		control_traffic();

	switch(state) {
	case 0:
		debug_mat.instance();
		debug_mat->set_flag(SpatialMaterial::FLAG_DISABLE_DEPTH_TEST, true);
		debug_mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
		debug_mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
		instance = VS::get_singleton()->instance_create();
		VS::get_singleton()->instance_set_scenario(instance, tree->get_root()->get_world()->get_scenario());
		immediate = VS::get_singleton()->immediate_create();
		VisualServer::get_singleton()->instance_set_base(instance, immediate);
		VS::get_singleton()->immediate_set_material(immediate, debug_mat->get_rid());
		tree->connect("idle_frame", this, "idle_runner");
		state = 1;
		break;
	case 1:
		if (list_cooldown > 0.0f) {
			list_cooldown -= delta;
			return;
		}
		tree->get_nodes_in_group("traffic_spawn", &spawner_list);
		spawner_e = spawner_list.front();
		list_cooldown = 0.1f;
		if (spawner_list.size() > 0)
			state = 2;
		spawn_cooldown = 0.0f;
		break;
	case 2:
		if (scenes.size() == 0)
			return;
		if (!spawner_e) {
			state = 1;
			return;
		}
		if (spawn_cooldown > 0.0f) {
			spawn_cooldown -= delta;
			return;
		}
		view = tree->get_root();
		if (!view)
			return;
		camera = view->get_camera();
		if (!camera)
			return;
		cam_xform = camera->get_global_transform();
		while (spawner_e) {
			float dst;
			VehicleBody *vb;
			Node *nscene;
			Spatial *sscene;
			Transform xform;
			Ref<PackedScene> scene;
			Vector3 check_coordinates;

			after = OS::get_singleton()->get_ticks_usec();
			if (after - before > 2000ULL)
				break;
			Node *spawner;
			Spatial *obj;
			spawner = spawner_e->get();
			if (!spawner)
				goto pass_loop;
			if (!spawner->is_in_group("spawn"))
				goto pass_loop;
			obj = Object::cast_to<Spatial>(spawner);
			if (!obj)
				goto pass_loop;
			if (obj->has_meta("cooldown")) {
				float cooldown = obj->get_meta("cooldown");
				cooldown -= delta;
				if (cooldown >= 0.0f) {
					obj->set_meta("cooldown", cooldown);
					goto pass_loop;
				} else
					obj->remove_meta("cooldown");
			}
			xform = obj->get_global_transform();
			check_coordinates = cam_xform.xform_inv(xform.origin);
			if (check_coordinates.z > -15.0f && check_coordinates.z < min_spawn_distance && fabsf(check_coordinates.x) < 20.0  )
				goto pass_loop;
			dst = xform.origin.distance_squared_to(cam_xform.origin);
			if (check_coordinates.z > max_spawn_distance || check_coordinates.z < -50.0f || fabsf(check_coordinates.x) > 50.0f)
				goto pass_loop;
			printf("traffic: spawning scene\n");
			scene = scenes[rnd->randi() % scenes.size()];
			nscene = scene->instance();
			sscene = Object::cast_to<Spatial>(nscene);
			view->add_child(sscene);
			sscene->set_global_transform(xform);
			sscene->add_to_group("traffic_vehicle");
			sscene->set("parked", false);
			vb = Object::cast_to<VehicleBody>(nscene);
			if (vb) {
				vb->set_engine_force(2500.0f);
				vb->set_steering(0);
				RID space = PhysicsServer::get_singleton()->body_get_space(vb->get_rid());
				vb->set_meta("space", space);
				PhysicsServer::get_singleton()->body_set_space(vb->get_rid(), RID());
			}
			obj->set_meta("cooldown", 30.0f + rnd->randf() * 30.0);
			spawn_cooldown = 0.2f;
pass_loop:
			spawner_e = spawner_e->next();
		}
		break;
	};
}
void Traffic::debug_traffic()
{
	SceneTree *tree = SceneTree::get_singleton();
	List<Node *> traffic_vehicles;
	List<Node *>::Element *e;
	tree->get_nodes_in_group("traffic_vehicle", &traffic_vehicles);
	VS::get_singleton()->immediate_clear(immediate);
	if (traffic_vehicles.size() == 0)
		return;
	printf("traffic: vehicles: %d\n", traffic_vehicles.size());
	VS::get_singleton()->immediate_begin(immediate, VS::PRIMITIVE_LINES);
	for (e = traffic_vehicles.front(); e; e = e->next()) {
		Node *vnode = e->get();
		if (!vnode)
			continue;
		Spatial *vsp = Object::cast_to<Spatial>(vnode);
		if (!vsp)
			continue;
		Transform vxform = vsp->get_global_transform();
		VS::get_singleton()->immediate_color(immediate, Color(1, 0, 0, 1));
		VS::get_singleton()->immediate_vertex(immediate, vxform.origin);
		VS::get_singleton()->immediate_color(immediate, Color(0, 0, 1, 1));
		VS::get_singleton()->immediate_vertex(immediate, vxform.origin + Vector3(0, 30, 0));
		if (vsp->has_meta("next_target")) {
			VS::get_singleton()->immediate_color(immediate, Color(1, 0, 0, 1));
			VS::get_singleton()->immediate_vertex(immediate, vxform.origin);
			VS::get_singleton()->immediate_color(immediate, Color(1, 0, 0, 1));
			VS::get_singleton()->immediate_vertex(immediate, vsp->get_meta("next_target"));
		}
		if (vsp->has_meta("target")) {
			VS::get_singleton()->immediate_color(immediate, Color(1, 0, 1, 1));
			VS::get_singleton()->immediate_vertex(immediate, vxform.origin);
			VS::get_singleton()->immediate_color(immediate, Color(1, 0, 1, 1));
			VS::get_singleton()->immediate_vertex(immediate, vsp->get_meta("target"));
		}
		if (vsp->has_meta("x_target")) {
			VS::get_singleton()->immediate_color(immediate, Color(0, 1, 0, 1));
			VS::get_singleton()->immediate_vertex(immediate, vxform.origin);
			VS::get_singleton()->immediate_color(immediate, Color(0, 1, 0, 1));
			Vector3 vp = vsp->get_meta("x_target");
			VS::get_singleton()->immediate_vertex(immediate, vxform.xform(vp));
		}
		if (vsp->has_meta("vel")) {
			VS::get_singleton()->immediate_color(immediate, Color(1, 1, 1, 1));
			VS::get_singleton()->immediate_vertex(immediate, vxform.origin);
			VS::get_singleton()->immediate_color(immediate, Color(0, 0, 1, 1));
			Vector3 vp = vsp->get_meta("vel");
			if (vp.length_squared() == 0.0f) {
				vp += Vector3(0, 2, 0);
			}
			VS::get_singleton()->immediate_vertex(immediate, vxform.xform(vp));
		}
		if (vsp->has_meta("curve")) {
			Ref<Curve3D> curve = vsp->get_meta("curve");
			if (curve->get_point_count() > 0) {
				float offt = curve->get_closest_offset(vxform.origin);
				float plength = curve->get_baked_length();
				float pstart = CLAMP(offt - 4.0f, 0.0f, plength);
				Vector3 pt = curve->interpolate_baked(offt, false);
				VS::get_singleton()->immediate_color(immediate, Color(1, 1, 1, 1));
				VS::get_singleton()->immediate_vertex(immediate, vxform.origin);
				VS::get_singleton()->immediate_color(immediate, Color(0, 1, 0, 1));
				VS::get_singleton()->immediate_vertex(immediate, pt);
				while (pstart < offt + 8.0 && pstart < plength) {
					Vector3 opt = curve->interpolate_baked(pstart, false);
					float c = (pstart - offt + 4.0) / 16.0f;
					c = CLAMP(c, 0.0f, 1.0f);
					VS::get_singleton()->immediate_color(immediate, Color(1, c, c, 1));
					VS::get_singleton()->immediate_vertex(immediate, opt);
					VS::get_singleton()->immediate_color(immediate, Color(1, c, c, 1));
					VS::get_singleton()->immediate_vertex(immediate, opt + Vector3(0, 10, 0));
					pstart += 1.0f;
				}
			}
		}
	}
	VS::get_singleton()->immediate_end(immediate);
}
void Traffic::control_traffic()
{
	Viewport *view;
	Camera *camera;
	Transform cam_xform;
	SceneTree *tree = SceneTree::get_singleton();
	List<Node *> traffic_vehicles;
	List<Node *>::Element *e;
	tree->get_nodes_in_group("traffic_vehicle", &traffic_vehicles);
	view = tree->get_root();
	camera = view->get_camera();
	cam_xform = camera->get_global_transform();
	for (e = traffic_vehicles.front(); e; e = e->next()) {
		Node *node = e->get();
		Spatial *sp = Object::cast_to<Spatial>(node);
		Transform xform = sp->get_global_transform();
		Vector3 check = cam_xform.xform_inv(xform.origin);
		/* TODO: use AABB instead */
		if (fabsf(check.x) > delete_distance.x ||
				check.z > delete_distance.z ||
				check.z < delete_distance.y)
			sp->queue_delete();
		else if (fabsf(check.x) > physics_distance.x + 1.0f ||
				check.z > physics_distance.z + 1.0f ||
				check.z < physics_distance.y - 1) {
			if (!sp->has_meta("space"))
				vehicle_disable_physics(sp);
		} else if (fabsf(check.x) < physics_distance.x - 1.0f ||
				check.z < physics_distance.z - 1.0f ||
				check.z > physics_distance.y + 1) {
			if (sp->has_meta("space"))
				vehicle_enable_physics(sp);
		}
	}
}

void Traffic::vehicle_enable_physics(Spatial *obj)
{
	RigidBody *body = Object::cast_to<RigidBody>(obj);
	if (!body)
		return;

	RID space = obj->get_meta("space");
	PhysicsServer::get_singleton()->body_set_space(body->get_rid(), space);
	obj->remove_meta("space");
	if (obj->has_meta("velocity")) {
		Vector3 velocity = obj->get_meta("velocity");
		body->set_linear_velocity(velocity);
	}
}

void Traffic::vehicle_disable_physics(Spatial *obj)
{
	RigidBody *body = Object::cast_to<RigidBody>(obj);
	if (!body)
		return;
	RID space = PhysicsServer::get_singleton()->body_get_space(body->get_rid());
	Vector3 velocity = body->get_linear_velocity();
	obj->set_meta("space", space);
	obj->set_meta("velocity", velocity);
	PhysicsServer::get_singleton()->body_set_space(body->get_rid(), RID());
}

static Traffic *g_traffic_data = NULL;
Traffic *Traffic::get_singleton()
{
	return g_traffic_data;
}
void Traffic::create_singleton()
{
	g_traffic_data = memnew(Traffic);
}
void Traffic::destroy_singleton()
{
	memdelete(g_traffic_data);
	g_traffic_data = NULL;
}