#include <cassert>
#include <cstdio>
#include <core/list.h>
#include <scene/3d/spatial.h>
#include <scene/3d/physics_body.h>
#include <scene/3d/immediate_geometry.h>
#include <scene/3d/vehicle_body.h>
#include <scene/animation/animation_tree.h>
#include <scene/animation/animation_node_state_machine.h>
#include <scene/resources/mesh.h>
#include <modules/detour/crowd.h>
#include "smart_object.h"
#include "characters.h"


Characters_::Characters_() : query(memnew(DetourNavigationQuery)),
	initialized(false), debug(NULL),
	crowd(NULL)
{
	smm = NULL;
	no_navmesh = true;
}
AnimationTree *Characters_::get_animation_tree(const Node *npc) const
{
	if (!npc->has_meta("animation_tree"))
		return NULL;
	Object * obj = npc->get_meta("animation_tree");
	if (!obj)
		return NULL;
	AnimationTree *result = Object::cast_to<AnimationTree>(obj);
	return result;
}
bool Characters_::handle_cmdq(Node *npc)
{
	Array cmds, cmd;
	bool handled = false;
	if (npc->has_meta("cmdqueue"))
		cmds = npc->get_meta("cmdqueue");
	AnimationTree *anim = get_animation_tree(npc);
	if (cmds.size() > 0) {
		cmd = cmds[0];
		if (cmd[0] == "anim_state") {
			animation_node_travel(npc, cmd[1]);
			cmds.pop_front();
			handled = true;
		} else if (cmd[0] == "anim_param") {
			String p = "parameters/";
			p += String(cmd[1]);
			anim->set(p, cmd[2]);
			cmds.pop_front();
			handled = true;
		} else if (cmd[0] == "delay") {
			int d = cmd[1];
			cmds.pop_front();
			if (d > 0) {
				d--;
				Array ncmd;
				ncmd.push_back("delay");
				ncmd.push_back(d);
				cmds.push_front(ncmd);
			}
			handled = true;
		} else if (cmd[0] == "jump_to_obj") {
			Object *obj = cmd[1];
			Spatial *sp = Object::cast_to<Spatial>(obj);
			Spatial *npc_sp = Object::cast_to<Spatial>(npc);
			Transform g = sp->get_global_transform();
			npc_sp->set_global_transform(g);
			Transform orientation = npc->get_meta("orientation");
			orientation.basis = g.basis;
			npc->set_meta("orientation", orientation);
			cmds.pop_front();
			handled = true;
		} else if (cmd[0] == "jump_to_obj_rot") {
			Object *obj = cmd[1];
			Spatial *sp = Object::cast_to<Spatial>(obj);
			Spatial *npc_sp = Object::cast_to<Spatial>(npc);
			Transform xform;
			Transform g = sp->get_global_transform();
			xform.origin = g.origin;
			xform.basis = g.basis.rotated(Vector3(0, 1, 0), Math_PI);
			npc_sp->set_global_transform(xform);
			Transform orientation = npc->get_meta("orientation");
			orientation.basis = xform.basis;
			npc->set_meta("orientation", orientation);
			cmds.pop_front();
			handled = true;
		}
	}

	if (cmds.size() == 0 && npc->has_meta("cmdqueue"))
		npc->remove_meta("cmdqueue");
	else if (cmds.size() > 0 && handled)
		npc->set_meta("cmdqueue", cmds);
	return handled;
}
String Characters_::get_animation_node(const Node *npc)
{
	AnimationTree *anim_tree = get_animation_tree(npc);
	if (!anim_tree)
		return "";
	Ref<AnimationNodeStateMachinePlayback> playback;
	playback = anim_tree->get("parameters/state/playback");
	if (!playback.ptr())
		return "";
	return playback->get_current_node();
}
void Characters_::animation_node_travel(Node *npc, const String &anim)
{
	AnimationTree *anim_tree = get_animation_tree(npc);
	if (!anim_tree)
		return;
	Ref<AnimationNodeStateMachinePlayback> playback;
	playback = anim_tree->get("parameters/state/playback");
	if (!playback.ptr())
		return;
	playback->travel(anim);
}
void Characters_::set_walk_speed(Node *npc, float speed, float strafe)
{
	AnimationTree *anim_tree = get_animation_tree(npc);
	if (!anim_tree)
		return;
	Vector2 spd(speed, strafe);
	anim_tree->set("parameters/state/locomotion/loc/blend_position", spd);
}
float Characters_::get_walk_speed(const Node *npc) const
{
	AnimationTree *anim_tree = get_animation_tree(npc);
	if (!anim_tree)
		return 0.0f;
	Vector2 spd = anim_tree->get("parameters/state/locomotion/loc/blend_position");
	return spd.x;
}
void Characters_::rotate_to_agent(Spatial *obj)
{
	if (!obj->has_meta("agent_id"))
		return;
	Transform orientation = obj->get_meta("orientation");
#if 0
	if (get_animation_node(obj).begins_with("turn_"))
		return;
#endif
	Vector3 agent_velocity = obj->get_meta("agent_velocity");
	Vector3 agent_position = obj->get_meta("agent_position");
	Transform g = obj->get_global_transform();
	Vector3 to = agent_position - g.origin + agent_velocity;
	Vector3 dir = orientation.xform(Vector3(0.0f, 0.0f, -1.0f));
	to.y = 0.0f;
	Vector2 to2(to.x, to.z);
	Vector2 dir2(dir.x, dir.z);
	float angle = dir2.angle_to(to2);
	if (fabs(angle) < Math_PI / 8.0f || to.length_squared() < 1.9f * 1.9f) {
		if (get_animation_node(obj).begins_with("turn_"))
			animation_node_travel(obj, "locomotion");
		Transform rot = Transform().looking_at(to, Vector3(0.0f, 1.0f, 0.0f));
		orientation.basis = rot.basis;
		obj->set_meta("orientation", orientation);
		g.basis = orientation.basis;
		obj->set_global_transform(g);
	} else {
		if (get_animation_node(obj).begins_with("turn_"))
			return;
		Vector3 sk = orientation.xform_inv(to);
		if (sk.z > 1.9f)
			animation_node_travel(obj, "turn_left");
		/* CCW */
		else if (sk.x > 0.0f)
			animation_node_travel(obj, "turn_left");
		/* CW */
		else if (sk.x < 0.0f)
			animation_node_travel(obj, "turn_right");
	}
}
void Characters_::speed_to_agent(Spatial *obj)
{
	if (!crowd)
		return;
	float delta = get_physics_process_delta_time();
	float cur_speed = get_walk_speed(obj);
	float new_speed;
	Vector3 agent_velocity = obj->get_meta("agent_velocity");
	Vector3 agent_position = obj->get_meta("agent_position");
	int agent_id = obj->get_meta("agent_id");
	bool update_speed = false;
	Transform g = obj->get_global_transform();
#if 0
	KinematicBody *kb = Object::cast_to<KinematicBody>(obj);
#endif
	if (obj->has_meta("cur_speed")) {
		float tmp = obj->get_meta("cur_speed");
		if (cur_speed != tmp) {
#if 0
			printf("something is very wrong\n");
#endif
			cur_speed = cur_speed + (tmp - cur_speed) * delta;
			set_walk_speed(obj, cur_speed, 0.0f);
		}
	}
	if (agent_position.distance_squared_to(g.origin) > 0.0f) {
		agent_velocity = agent_position - g.origin + agent_velocity;
		Vector3 apos = g.xform_inv(agent_position);
		float dinc = -apos.z;
		float xinc = apos.x;
		// printf("position mismatch %f\n", dinc);
		if (fabs(dinc) > 0.2f || fabs(xinc) > 0.2f)
			crowd->reset_agent(agent_id);
		Vector3 v = agent_velocity;
		v.y = 0.0f;
		float spd = v.length();
#if 0
		cur_speed = get_walk_speed(obj);
		if (obj->has_meta("cur_speed"))
			cur_speed = obj->get_meta("cur_speed");
#endif

		if (spd > 40.0f)
			new_speed = 0.5f;
		else if (spd > 0.0f)
			new_speed = 0.1f + 0.4f * (spd / 60.0f);
		else
			new_speed = 0.0f;
#if 0
		if (dinc > 0.15f)
			new_speed += 0.1f * delta;
		if (dinc < -0.15f)
			new_speed -= 0.1f * delta;
#endif
		if (new_speed > 0.0f)
			new_speed = cur_speed +
				(new_speed - cur_speed) * delta;
		new_speed = CLAMP(new_speed, 0.0f, 1.0f);
		if (new_speed != cur_speed) {
#if 0
			printf("cur_speed = %f %f spd = %f\n", cur_speed, dinc, spd);
			printf("new speed = %f cur_speed = %f\n", new_speed, cur_speed);
#endif
			if (new_speed == 0.0f && cur_speed > 0.0f) {
				animation_node_travel(obj, "stop_walking");
				printf("stopping 1\n");
				obj->set_meta("cur_speed", 0.0f);
				cur_speed = new_speed;
				update_speed = true;
			}
			else if (cur_speed == 0.0f && new_speed > 0.0f) {
				animation_node_travel(obj, "start_walking");
				printf("starting 1\n");
				obj->set_meta("cur_speed", new_speed);
				cur_speed = new_speed;
				update_speed = true;
			} else if (cur_speed >= 0.0f && new_speed > 0.0f) {
				obj->set_meta("cur_speed", new_speed);
				cur_speed = new_speed;
				update_speed = true;
			}
		}
//		printf("cur_speed = %f speed = %f new_speed = %f\n", cur_speed, get_walk_speed(obj), new_speed);
		if (new_speed > 0.0f) {
			if (dinc > 0.15f) {
				new_speed = CLAMP(new_speed + delta * 2.5f, 0.1f, 1.0f);
				update_speed = true;
			} else if (dinc < -0.15f) {
				new_speed = CLAMP(new_speed - delta * 2.5f, 0.1f, 1.0f);
				update_speed = true;
			}
		}
	}
	if (update_speed)
		set_walk_speed(obj, new_speed, 0.0f);
	if (cur_speed > 0.0f)
		assert(get_walk_speed(obj) > 0.0f);
}
bool Characters_::has_arrived(Object *obj)
{
	if (!crowd)
		return false;
	Spatial *sp = Object::cast_to<Spatial>(obj);
	if (!obj->has_meta("agent_id"))
		return false;
	if (!obj->has_meta("_target"))
		return false;
	int agent_id = obj->get_meta("agent_id");
	bool crowd_arrived = crowd->has_arrived(agent_id);
	if (crowd_arrived) {
		Vector3 cpos = obj->get_meta("agent_position");
		Vector3 spos = sp->get_global_transform().origin;
		if (cpos.distance_squared_to(spos) <
				arrive_precision * arrive_precision)
			return true;
	}
	return false;
}
void Characters_::update_arrived(Object *obj)
{
	if (!crowd)
		return;
	Spatial *sp = Object::cast_to<Spatial>(obj);
	int agent_id = obj->get_meta("agent_id");
	if (obj->has_meta("climb"))
		obj->remove_meta("climb");
	crowd->clear_agent_target(agent_id);
	if (!sp)
		return;
	printf("stopping 2\n");
	set_walk_speed(sp, 0.0f, 0.0f);
	obj->set_meta("cur_speed", 0.0f);
	animation_node_travel(sp, "stop_walking");
	if (obj->has_meta("_target_jumpto")) {
		Transform xform = obj->get_meta("_target_jumpto");
		sp->set_global_transform(xform);
		obj->remove_meta("_target_jumpto");
	}
	if (obj->has_meta("_target_animation")) {
		String anim = obj->get_meta("_target_animation");
		animation_node_travel(sp, anim);
		obj->remove_meta("_target_animation");
	}
	if (obj->has_meta("_target_cmdq")) {
		Array cmds;
		if (obj->has_meta("cmdqueue"))
			cmds = obj->get_meta("cmdqueue");
		Array tcmds = obj->get_meta("_target_cmdq");
		cmds.append_array(tcmds);
		obj->set_meta("cmdqueue", cmds);
		obj->remove_meta("_target_cmdq");
	}
}
void Characters_::character_physics(Object *obj)
{
	float delta = get_physics_process_delta_time();
	Vector3 h_velocity, velocity;
	AnimationTree *animtree;
	Transform orientation, root_motion, g;
	Object *tmp;

	tmp = obj->get_meta("animation_tree");
	animtree = Object::cast_to<AnimationTree>(tmp);

	orientation = obj->get_meta("orientation");
	root_motion = animtree->get_root_motion_transform();
	root_motion.origin = root_motion_mod.xform(root_motion.origin);
	orientation *= root_motion;
	h_velocity = orientation.origin / delta;
	velocity = h_velocity;
	KinematicBody *kb = Object::cast_to<KinematicBody>(obj);
	if (kb) {
		bool go = false;
		if (!obj->has_meta("vehicle") && !obj->has_meta("cmdqueue") /*&& !obj->has_meta("smart_object")*/) {
			go = true;
		} else if (obj->has_meta("cmdqueue") && obj->has_meta("cmdq_walk")) {
			go = true;
		} else if (obj->has_meta("cmdqueue") && obj->has_meta("climb")) {
			go = true;
		}
		if (!kb->is_on_floor() && !obj->has_meta("climb"))
			velocity += Vector3(0.0f, -9.8f, 0.0f);
		if (go)
			velocity = kb->move_and_slide(velocity, Vector3(0.0f, 1.0f, 0.0f), true, 4, 0.785f, false);
	}
	orientation.origin = Vector3();
	orientation.orthonormalize();
	obj->set_meta("orientation", orientation);
	Spatial *sp = Object::cast_to<Spatial>(obj);
	if (sp) {
		g = sp->get_global_transform();
		g.basis = orientation.basis;
		sp->set_global_transform(g);

		if (obj->has_meta("vehicle")) {
			tmp = obj->get_meta("vehicle");
			VehicleBody *vehicle = Object::cast_to<VehicleBody>(tmp);
			if (obj->has_meta("vehicle_offset")) {
				Transform xform = obj->get_meta("vehicle_offset");
				g = vehicle->get_global_transform() * xform;
				sp->set_global_transform(g);
			}
		}
	}
	Node *node = Object::cast_to<Node>(obj);
	if (node) {
		if (node->has_node(String("blackboard"))) {
			Node *bb = node->get_node(String("blackboard"));
			bb->call("set", "velocity", velocity);
		}
	}
	if (!crowd)
		return;
	if (obj->has_meta("smart_object"))
		return;
	if (obj->has_meta("cmdqueue") && !obj->has_meta("cmdq_walk"))
		return;
	/* agent creation stuff */
	if (!obj->has_meta("agent_id")) {
		float agent_data_f[] = {0.35f, 1.5f, 1.8f, 40.0f};
		PoolVector<float> agent_data;
		agent_data.resize(4);
		memcpy(agent_data.write().ptr(), agent_data_f, sizeof(agent_data_f));
		crowd->add_agent(obj, 0, false, agent_data);
	}
	if (!obj->has_meta("agent_id"))
		return;
	if (!node)
		return;
	if (!obj->has_meta("_target"))
		return;
	float fwd_probe = forward_probe(node, 0.5f, 0.05f, 1.9f, 0.2f);
	bool climb = false;
	bool bumped = false;
	if (obj->has_meta("bumped")) {
		bumped = true;
		float remaining = obj->get_meta("bumped");
		remaining -= delta;
		if (remaining >= 0.0f)
			obj->set_meta("bumped", remaining);
		else
			obj->remove_meta("bumped");
	}
	if (fwd_probe < 0.1f) {
		if (obj->has_meta("climb"))
			climb = true;
		if (get_animation_node(node) != "climb1")
			climb = false;
	} else if (fwd_probe < 1.0f)
		climb = true;
	else if (fwd_probe >= 1.0f && !bumped) {
		int agent_id = obj->get_meta("agent_id");
		printf("bumped into wall %f\n", fwd_probe);
		bumped = true;
		printf("stopping 3\n");
		set_walk_speed(node, 0.0f, 0.0f);
		obj->set_meta("bumped", 3.0f);
		obj->set_meta("cur_speed", 0.0f);
		animation_node_travel(node, "stop_walking");
		crowd->reset_agent(agent_id);
	}
	if (climb && !obj->has_meta("climb")) {
		animation_node_travel(node, "climb1");
		obj->set_meta("climb", true);
	} else if (!climb && obj->has_meta("climb")) {
		animation_node_travel(node, "locomotion");
		obj->remove_meta("climb");
	}
	if (climb)
		return;
	/* agent stuff */
	Vector3 agent_velocity = obj->get_meta("agent_velocity");
	Vector3 agent_position = obj->get_meta("agent_position");
	g = sp->get_global_transform();
	if (!bumped)
		speed_to_agent(sp);
	if (((agent_position - g.origin) * delta + agent_velocity).length() == 0.0f) {
		float wspd = obj->get_meta("cur_speed");
		if (wspd >= 0.0f) {
			printf("stopping 4\n");
			set_walk_speed(node, 0.0f, 0.0f);
			animation_node_travel(node, "stop_walking");
			obj->set_meta("cur_speed", 0.0f);
		}
	} else
		rotate_to_agent(sp);
	if (has_arrived(obj)) {
		printf("ARRIVED\n");
		Vector3 where;
		update_arrived(obj);
		bool smart = false;
		SmartObject *sm = NULL;
		if (obj->has_meta("_target_node")) {
			Object *o = ObjectDB::get_instance(obj->get_meta("_target_node"));
			if (o) {
				sm = Object::cast_to<SmartObject>(o);
				if (sm)
					smart = true;
			}
		}
		if (!smart) {
			printf("ARRIVED simple\n");
			agent_walk_stop(obj);
			where = obj->get_meta("_target");
			emit_signal("arrived", obj, where);
			printf("arrived to %ls\n", String(where).c_str());
		} else {
			printf("ARRIVED smart\n");
			smm->arrived(sm, sp);
		}
	} else {
		if (obj->has_meta("_target_node")) {
			Node *tn = obj->get_meta("_target_node");
			if (tn) {
				PhysicsBody *pbt = Object::cast_to<PhysicsBody>(tn);
				if (pbt) {
					Vector3 target_cur = pbt->get_global_transform().origin;
					obj->set_meta("_target", target_cur);
				} else {
					Spatial *spt = Object::cast_to<Spatial>(tn);
					if (spt) {
						Vector3 target_cur = spt->get_global_transform().origin;
						obj->set_meta("_target", target_cur);
					}
				}
			}
		}
	}
}
void Characters_::agent_walk_stop(Object *obj)
{
	if (obj->has_meta("_target"))
		obj->remove_meta("_target");
	if (obj->has_meta("_target_node"))
		obj->remove_meta("_target_node");
}
void Characters_::walkto_agent_node(Node *ch, const Node *target)
{
	if (!ch->has_meta("agent_id"))
		return;
	ch->set_meta("_target_node", target->get_instance_id());
	const Spatial *sp = Object::cast_to<Spatial>(target);
	if (sp) {
		Vector3 tpos = sp->get_global_transform().origin;
		walkto_agent(ch, tpos);
	}
}
void Characters_::walkto_agent(Node *ch, const Vector3 &target)
{
	if (!crowd)
		return;
	if (ch->has_meta("_target")) {
		Vector3 otarget = ch->get_meta("_target");
		if (otarget == target)
			return;
	}
	assert(ch->has_meta("agent_id"));
	int agent_id = ch->get_meta("agent_id");
	ch->set_meta("_target", target);
	crowd->set_agent_target_position(agent_id, target);
	printf("target set to %ls\n", String(target).c_str());
}
void Characters_::_notification(int p_what)
{
	List<Node *> char_node_list;
	List<Node *> frozen_node_list;
	List<Node *>::Element *e;
	switch(p_what) {
	case NOTIFICATION_PROCESS:
		if (!initialized)
			return;
		get_tree()->get_nodes_in_group("character", &char_node_list);
		get_tree()->get_nodes_in_group("frozen", &frozen_node_list);
		if (debug) {
			debug->clear();
			debug->begin(Mesh::PRIMITIVE_LINES);
			debug->set_color(Color(1, 0, 0, 1));
		}
		for (e = char_node_list.front(); e; e = e->next()) {
			if (debug)
				debug->set_color(Color(1, 0, 0, 1));
			Node *ch = e->get();
			if (!ch->has_meta("animation_tree"))
				continue;
			process_character(ch, false);
			if (debug) {
				Spatial *sp = Object::cast_to<Spatial>(ch);
				debug->set_color(Color(1, 0.5f, 1, 1));
				Transform xf = sp->get_global_transform();
				Vector3 p1 = xf.origin;
				// Vector3 b = -xf.basis[2];
				// b.x = -b.x;
				xf.origin = Vector3();
				Vector3 b = xf.xform(Vector3(0.0f, 0.0f, -1.0f));
				Vector3 p2 = p1 + b * 2.0f;
				debug->add_vertex(p1);
				debug->add_vertex(p2);
				if (ch->has_meta("agent_velocity")) {
					Vector3 p3 = p1 + ch->get_meta("agent_velocity");
					debug->set_color(Color(0.5f, 1, 0.5f, 1));
					debug->add_vertex(p1);
					debug->add_vertex(p3);
				}
			}
		}
		if (debug)
			debug->set_color(Color(1, 0, 0, 1));
		for (e = frozen_node_list.front(); e; e = e->next()) {
			Node *ch = e->get();
			if (!ch->has_meta("animation_node"))
				continue;
			process_character(ch, true);
		}
		if (debug)
			debug->end();
		break;
	case NOTIFICATION_PHYSICS_PROCESS:
		if (!initialized)
			return;
		get_tree()->get_nodes_in_group("character", &char_node_list);
#if 0
		if (debug) {
			debug->clear();
			debug->begin(Mesh::PRIMITIVE_LINES);
			debug->set_color(Color(1, 0, 0, 1));
		}
#endif
		for (e = char_node_list.front(); e; e = e->next()) {
			Node *ch = e->get();
			if (!ch->has_meta("animation_tree"))
				continue;
			if (!ch->has_meta("orientation"))
				continue;
			printf("running physics for %p\n", ch);
			character_physics(ch);
			Spatial *sp = Object::cast_to<Spatial>(ch);
			Vector3 direction = sp->get_global_transform().xform(Vector3(0, 0, -1));
			ch->set_meta("direction", direction);
		}
#if 0
		if (debug)
			debug->end();
#endif
		break;
	case NOTIFICATION_READY:
		arrive_precision = GLOBAL_DEF("ai/locomotion/arrive_precision", 0.2f);
		set_process(true);
		set_physics_process(true);
		smm = memnew(SmartObjectManager);
		add_child(smm);
		break;
	}
}
void Characters_::_bind_methods()
{
	ClassDB::bind_method(D_METHOD("walkto", "ch", "target"), &Characters_::walkto);
	ClassDB::bind_method(D_METHOD("walkto_node", "ch", "target"), &Characters_::walkto_node);
	ClassDB::bind_method(D_METHOD("set_navmesh", "mesh", "xform"), &Characters_::set_navmesh);
	ClassDB::bind_method(D_METHOD("forward_probe", "body", "lookahead", "start_height", "end_height", "height_step"), &Characters_::forward_probe);
	ClassDB::bind_method(D_METHOD("get_animation_node", "npc"), &Characters_::get_animation_node);
	ClassDB::bind_method(D_METHOD("handle_cmdq", "npc"), &Characters_::handle_cmdq);
	ClassDB::bind_method(D_METHOD("animation_node_travel", "npc", "anim"), &Characters_::animation_node_travel);
	ClassDB::bind_method(D_METHOD("set_walk_speed", "npc", "speed", "strafe"), &Characters_::set_walk_speed);
	ClassDB::bind_method(D_METHOD("get_walk_speed", "npc"), &Characters_::get_walk_speed);
	ClassDB::bind_method(D_METHOD("set_crowd", "crowd"), &Characters_::set_crowd_);
	ClassDB::bind_method(D_METHOD("get_crowd"), &Characters_::get_crowd);
	ClassDB::bind_method(D_METHOD("walkto_agent", "ch", "target"), &Characters_::walkto_agent);
	ClassDB::bind_method(D_METHOD("walkto_agent_node", "ch", "target"), &Characters_::walkto_agent_node);
	ClassDB::bind_method(D_METHOD("character_physics", "obj"), &Characters_::character_physics);
	ClassDB::bind_method(D_METHOD("set_root_motion_mod", "xform"), &Characters_::set_root_motion_mod);
	ClassDB::bind_method(D_METHOD("get_root_motion_mod"), &Characters_::get_root_motion_mod);
	ADD_SIGNAL(MethodInfo("arrived", PropertyInfo(Variant::OBJECT, "obj"), PropertyInfo(Variant::VECTOR3, "where")));
}

void Characters_::set_root_motion_mod(const Transform &xform)
{
	root_motion_mod = xform;
}
Transform Characters_::get_root_motion_mod() const
{
	return root_motion_mod;
}

void Characters_::process_frozen_character(Node *npc, const Vector3 &tposition)
{
	float delta = npc->get_process_delta_time();
	Vector3 tgt = tposition;
	Spatial *sp = Object::cast_to<Spatial>(npc);
	assert(sp);
	Vector3 position = sp->get_global_transform().origin;
	float dst = position.distance_squared_to(tgt);
	tgt.y = position.y;
	float tconst = delta / dst;
	Transform xform = sp->get_global_transform();
	xform = xform.looking_at(tgt, Vector3(0, 1, 0));
	xform.origin = position.linear_interpolate(tgt, tconst);
	sp->set_global_transform(xform);
}

void Characters_::process_normal_character(Node *npc, const Vector3 &tposition)
{
#if 0
	Spatial *sp = Object::cast_to<Spatial>(npc);
	assert(sp);
	float delta = npc->get_process_delta_time();
	Vector3 tgt = tposition;
	Vector3 position = sp->get_global_transform().origin;
	tgt.y = position.y;
#endif
}

void Characters_::process_character(Node *node, bool frozen)
{
	int id = node->get_instance_id();
	Spatial *sp = Object::cast_to<Spatial>(node);
	if (!sp)
		return;
	Vector3 target;
	Vector3 position = sp->get_global_transform().origin;
	if (static_targets.has(id))
		target = static_targets[id];
	else if (dynamic_targets.has(id)) {
		int target_id = dynamic_targets[id];
		Object * obj = ObjectDB::get_instance(target_id);
		if (!obj) {
			dynamic_targets.erase(id);
			return;
		}
		Spatial *target_node = Object::cast_to<Spatial>(obj);
		if (!target_node) {
			dynamic_targets.erase(id);
			return;
		}
		target = target_node->get_global_transform().origin;
	} else /* no target - no problem */
		return;
	float distance = position.distance_squared_to(target);
	int i;
	if (!paths.has(id)) {
		Vector<Vector3> points;
		Vector<int> flags;
		if (!no_navmesh) {
			Vector3 start = query->nearest_point(position, Vector3(1, 1, 1), filter);
			Vector3 end = query->nearest_point(target, Vector3(1, 1, 1), filter);
			query->find_path_array(start, end, Vector3(1, 1, 1), filter, points, flags);
			assert(points.size() > 0);
			paths[id] = points;
		} else 
			paths[id] = Vector<Vector3>();
	}
	if (debug)
		for (i = 0; i < paths[id].size() - 1; i++) {
			debug->add_vertex(paths[id][i]);
			debug->add_vertex(paths[id][i + 1]);
		}
	if (distance < 0.5) {
		if (static_targets.has(id))
			static_targets.erase(id);
		if (dynamic_targets.has(id))
			dynamic_targets.erase(id);
		if (paths.has(id))
			paths.erase(id);
		emit_signal("arrived", node);
	} else if (frozen) {
		Vector3 tgt = target;
		if (paths[id].size() > 0)
			tgt = paths[id][0];
		float dst = position.distance_squared_to(tgt);
		while (dst < 0.0001f && paths[id].size() > 0) {
			paths[id].erase(tgt);
			tgt = paths[id][0];
			dst = position.distance_squared_to(tgt);
		}
		process_frozen_character(node, tgt);

	} else {
		Vector3 tgt = target;
		if (paths[id].size() > 0)
			tgt = paths[id][0];
		float dst = position.distance_squared_to(tgt);
		while (dst < 0.0001f && paths[id].size() > 0) {
			paths[id].erase(tgt);
			tgt = paths[id][0];
			dst = position.distance_squared_to(tgt);
		}
		process_normal_character(node, tgt);
	}
}

Characters_::~Characters_()
{
	memdelete(query);
}

void Characters_::walkto(const Node *ch, const Vector3 &target)
{
	static_targets[ch->get_instance_id()] = target;
}

void Characters_::walkto_node(const Node *ch, const Node *target)
{
	dynamic_targets[ch->get_instance_id()] = target->get_instance_id();
}

void Characters_::set_navmesh(Ref<DetourNavigationMesh> mesh, const Transform &xform)
{
	if (mesh.is_null()) {
		no_navmesh = true;
		initialized = true;
		return;
	}
	query->init(mesh, xform);
	filter.instance();
	debug = memnew(ImmediateGeometry);
	add_child(debug);
	no_navmesh = false;
	initialized = true;
	Ref<SpatialMaterial> mat;
	mat.instance();
	mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
	mat->set_flag(SpatialMaterial::FLAG_DISABLE_DEPTH_TEST, true);
	mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
	debug->set_material_override(mat);
}
float Characters_::forward_probe(Node *body, float lookahead,
			float start_height, float end_height,
			float height_step)
{
	float result = 0.0f, d;
	Ref<World> world;
	PhysicsDirectSpaceState *space;
	PhysicsDirectSpaceState::RayResult rr;
	Set<RID> exclude;
	PhysicsBody *pbody = Object::cast_to<PhysicsBody>(body);
	assert(pbody);

	world = pbody->get_world();
	if (!world.ptr())
		return result;
	space = PhysicsServer::get_singleton()->space_get_direct_state(world->get_space());
	d = start_height;
	exclude.insert(pbody->get_rid());
	while (d < end_height) {
		Transform g = pbody->get_global_transform();
		Vector3 o = g.origin + Vector3(0, 1, 0) * d;
		Vector3 f = g.xform(Vector3(0, 0, -1) * lookahead + Vector3(0, 1, 0) * d);
		if (space->intersect_ray(o, f, rr, exclude)) {
			int against = rr.collider_id;
			Object * obj = ObjectDB::get_instance(against);
			if (obj) {
				Node *col = Object::cast_to<Node>(obj);
				if (col && !col->is_in_group("character"))
					result = d;
			}
		}
		d += height_step;
	}
	return result;
}

void Characters_::set_crowd(DetourCrowdManager *crowd)
{
	this->crowd = crowd;
}
void Characters_::set_crowd_(Node *crowd_node)
{
	DetourCrowdManager *crowd =
		Object::cast_to<DetourCrowdManager>(crowd_node);
	if (crowd)
		set_crowd(crowd);
}

DetourCrowdManager *Characters_::get_crowd() const
{
	return crowd;
}