#include <cstdio>
#include <cassert>
#include <core/engine.h>
#include <scene/animation/animation_tree.h>
#include <scene/animation/animation_node_state_machine.h>
#include <scene/animation/animation_blend_tree.h>
#include "smart_object.h"

SmartObject::SmartObject(): anim_state(""),
	anim_finish_state(""),
	enabled(false), teleport(false),
	distance(0.0f), state(STATE_INIT)
{
#ifdef TOOLS_ENABLED
	run_in_editor = false;
#endif
}

void SmartObject::_notification(int p_what)
{
}

void SmartObject::_bind_methods()
{
}
#ifdef TOOLS_ENABLED
void SmartObject::draw_debug()
{
	if (!character.ptr())
		return;
	Node *node = character->instance();
	if (!node)
		return;
	add_child(node);
	Spatial *sp = Object::cast_to<Spatial>(node);
	if (sp)
		sp->set_transform(Transform());
	AnimationTree *anim = find_animation_tree(node);
	assert(anim);
	node->set_meta("animation_tree", anim);
	if (anim_state.length() > 0)
		SmartObjectManager::play_animation(node, anim_state);
	else
		SmartObjectManager::play_animation(node, "locomotion");
	set_meta("debug", node);
}
void SmartObject::clear_debug()
{
	if (!has_meta("debug"))
		return;
	Node *node = get_meta("debug");
	if (node) {
		remove_child(node);
		node->queue_delete();
	}
	remove_meta("debug");
}
#endif

void SmartObject::_get_property_list(List<PropertyInfo> *p_list) const
{
#if TOOLS_ENABLED
	String animations;
	if (character.ptr()) {
		animations = get_animation_list();
	}
	if (animations.length() == 0)
		animations = "default";
#endif
	p_list->push_back(PropertyInfo(Variant::BOOL, "enabled"));
#if TOOLS_ENABLED
	p_list->push_back(PropertyInfo(Variant::STRING, "animation_state", PROPERTY_HINT_ENUM, animations));
	p_list->push_back(PropertyInfo(Variant::STRING, "animation_finish_state", PROPERTY_HINT_ENUM, animations));
#else 
	p_list->push_back(PropertyInfo(Variant::STRING, "animation_state", PROPERTY_HINT_NONE, ""));
	p_list->push_back(PropertyInfo(Variant::STRING, "animation_finish_state", PROPERTY_HINT_NONE, ""));
#endif
	p_list->push_back(PropertyInfo(Variant::BOOL, "teleport"));
	p_list->push_back(PropertyInfo(Variant::REAL, "distance"));
#ifdef TOOLS_ENABLED
	p_list->push_back(PropertyInfo(Variant::OBJECT, "character", PROPERTY_HINT_RESOURCE_TYPE, "PackedScene"));
	p_list->push_back(PropertyInfo(Variant::BOOL, "run_in_editor", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_INTERNAL | PROPERTY_USAGE_EDITOR));
#endif
}

bool SmartObject::_get(const StringName &p_name, Variant &r_ret) const
{
	if (p_name == "enabled") {
		r_ret = enabled;
		return true;
	} else if (p_name == "animation_state") {
		r_ret = anim_state;
		return true;
	} else if (p_name == "animation_finish_state") {
		r_ret = anim_finish_state;
		return true;
	} else if (p_name == "teleport") {
		r_ret = teleport;
		return true;
	} else if (p_name == "distance") {
		r_ret = distance;
		return true;
#ifdef TOOLS_ENABLED
	} else if (p_name == "character") {
		r_ret = character;
		return true;
	} else if (p_name == "run_in_editor") {
		r_ret = run_in_editor;
		return true;
#endif
	}
	return false;
}
#ifdef TOOLS_ENABLED
String SmartObject::dump_subnodes(Ref<AnimationNode> anode) const
{
	String animations = "";
	List<AnimationNode::ChildNode> child_nodes;
	anode->get_child_nodes(&child_nodes);
	List<AnimationNode::ChildNode>::Element *c;
	for (c = child_nodes.front(); c; c = c->next()) {
		AnimationNode::ChildNode cn = c->get();
		animations += cn.name;
		if (c->next())
			animations += ",";
	}
	return animations;
}

AnimationTree *SmartObject::find_animation_tree(Node *node) const
{
	int i;
	List<Node *> queue;
	queue.push_back(node);
	List<Node *>::Element *e;
	while(!queue.empty()) {
		e = queue.front();
		Node *item = e->get();
		queue.pop_front();
		AnimationTree *at = Object::cast_to<AnimationTree>(item);
		if (at) {
			if (!at->is_in_group("hair") &&
					!at->is_in_group("hair"))
				return at;
		}
		for (i = 0; i < item->get_child_count(); i++)
			queue.push_back(item->get_child(i));
	}
	return NULL;
}

String SmartObject::get_animation_list() const
{
	Node *node = character->instance();
	assert(node);
	String animations;
	if (node) {
		AnimationTree *anim = find_animation_tree(node);
		assert(anim);
		if (anim) {
			List<PropertyInfo> plist;
			Ref<AnimationNodeBlendTree> anim_root = get_anode<AnimationNodeBlendTree>(anim->get_tree_root());
			assert(anim_root.ptr());
			Ref<AnimationNodeStateMachine> anim_state = get_anode<AnimationNodeStateMachine>(anim_root->get_child_by_name("state"));
			assert(anim_state.ptr());
			animations = dump_subnodes(anim_state);
			printf("ANIMATIONS: %ls\n", animations.c_str());
#if 0
			anim_root->get_property_list(&plist);
			List<PropertyInfo>::Element *e;
			for (e = plist.front(); e; e = e->next()) {
				PropertyInfo pi = e->get();
				String name = pi.name;
				printf("node: %ls\n", name.c_str());
			}
#endif
		}
		node->queue_delete();
	}
	return animations;
}
#endif

bool SmartObject::_set(const StringName &p_name, const Variant &p_value)
{
	bool update = false;
	if (p_name == "enabled") {
		enabled = p_value;
		update = true;
	} else if (p_name == "animation_state") {
		anim_state = p_value;
		update = true;
	} else if (p_name == "animation_finish_state") {
		anim_finish_state = p_value;
		update = true;
	} else if (p_name == "teleport") {
		teleport = p_value;
		update = true;
	} else if (p_name == "distance") {
		distance = p_value;
		update = true;
#ifdef TOOLS_ENABLED
	} else if (p_name == "character") {
		character = p_value;
		update = true;
	} else if (p_name == "run_in_editor") {
		run_in_editor = p_value;
		update = true;
#endif
	}
	if (update) {
		if (enabled) {
			if (!is_in_group("_smart_object"))
				add_to_group("_smart_object");
		} else {
			if (is_in_group("_smart_object"))
				remove_from_group("_smart_object");
		}
		_change_notify();
#ifdef TOOLS_ENABLED
		if (Engine::get_singleton()->is_editor_hint()) {
			if (run_in_editor)
				draw_debug();
			else
				clear_debug();
		}
#endif
	}
	return update;
}

void SmartObjectManager::play_animation(Node *cnode, const String &anim_state)
{
	Node *anode = cnode->get_meta("animation_tree");
	assert(anode);
	AnimationTree *anim = Object::cast_to<AnimationTree>(anode);
	assert(anim);
	Ref<AnimationNodeStateMachinePlayback> playback;
	playback = anim->get("parameters/state/playback");
	assert(playback.ptr());
	playback->travel(anim_state);
	printf("ANIMATION play %ls\n", anim_state.c_str());
}

void SmartObjectManager::update_state(SmartObject *sm)
{
	if (sm->state == SmartObject::STATE_INIT)
		return;
	Object *obj = ObjectDB::get_instance(sm->npc_id);
	assert(obj);
	Node *ch = Object::cast_to<Node>(obj);
	assert(ch);
	Spatial *csp = Object::cast_to<Spatial>(obj);
	assert(csp);
	Transform oxform = sm->get_global_transform();
	Transform cxform = csp->get_global_transform();
	switch(sm->state) {
	case SmartObject::STATE_INIT:
		break;
	case SmartObject::STATE_SELECTED:
		printf("state == SELECTED\n");
		if (sm->teleport) {
			csp->set_global_transform(oxform);
			Transform orientation = ch->get_meta("orientation");
			orientation.basis = oxform.basis;
			ch->set_meta("orientation", orientation);
			printf("arrived: teleport\n");
			sm->state = SmartObject::STATE_ACTIVE;
		} else {
			if (!cxform.origin.is_equal_approx(oxform.origin))
				sm->state = SmartObject::STATE_CLOSE;
			else
				sm->state = SmartObject::STATE_ACTIVE;
		}
		break;
	case SmartObject::STATE_CLOSE:
		printf("state == CLOSE\n");
		if (!cxform.origin.is_equal_approx(oxform.origin)) {
			csp->set_global_transform(cxform.interpolate_with(oxform, get_process_delta_time()));
			printf("arrived: interpolate pos\n");
		} else
			sm->state = SmartObject::STATE_ACTIVE;
		break;
	case SmartObject::STATE_ACTIVE:
		printf("state == ACTIVE\n");
		printf("arrived: at position\n");
		printf("SMART object active\n");
		ch->set_meta("smart_object", Object::cast_to<Object>(sm));
		play_animation(ch, sm->anim_state);
		sm->state = SmartObject::STATE_PROGRESS;
		break;
	case SmartObject::STATE_PROGRESS:
		break;
	case SmartObject::STATE_FINISH:
		play_animation(ch, sm->anim_finish_state);
		ch->remove_meta("smart_object");
		ch->remove_meta("_target");
		ch->remove_meta("_target_node");
		sm->state = SmartObject::STATE_INIT;
		break;
	}
}

void SmartObjectManager::arrived(SmartObject *sm, Node *ch)
{
	assert(sm);
	assert(ch);
	assert(!ch->has_meta("smart_object"));
	Spatial *csp = Object::cast_to<Spatial>(ch);
	assert(csp);
	Node *onode = Object::cast_to<Node>(sm);
	assert(onode);
	Transform oxform = sm->get_global_transform();
	Transform cxform = csp->get_global_transform();
	switch(sm->state) {
	case SmartObject::STATE_INIT:
		printf("arrived: warning: STATE_INIT\n");
		sm->state = SmartObject::STATE_SELECTED;
		sm->npc_id = ch->get_instance_id();
		/* no break - transition to next state */
	case SmartObject::STATE_SELECTED:
	case SmartObject::STATE_CLOSE:
	case SmartObject::STATE_ACTIVE:
		update_state(sm);
		break;
	case SmartObject::STATE_PROGRESS:
		break;
	}
}

void SmartObjectManager::_notification(int p_what)
{
	List<Node *> char_node_list;
	List<Node *> smart_object_list;
	List<Node *>::Element *e, *c;
	switch(p_what) {
	case NOTIFICATION_PHYSICS_PROCESS:
		/* don't run in editor */
		if (Engine::get_singleton()->is_editor_hint())
			return;
		get_tree()->get_nodes_in_group("character", &char_node_list);
		get_tree()->get_nodes_in_group("_smart_object", &smart_object_list);
		for (e = char_node_list.front(); e; e = e->next()) {
			Node *cnode = e->get();
			if (!cnode->has_meta("_target"))
				continue;
			if (cnode->has_meta("smart_object"))
				continue;
			for (c = smart_object_list.front(); c; c = c->next()) {
				Node *onode = c->get();
				if (!onode)
					continue;
				if (cnode->has_meta("_target_node"))
					continue;
				Vector3 ctarget = cnode->get_meta("_target");
				SmartObject *obj = Object::cast_to<SmartObject>(onode);
				if (!obj)
					continue;
				cnode->set_meta("_target_node", onode);
#if 0
				if (ctarget.is_equal_approx(oxform.origin)) {
					float d = cxform.origin.distance_squared_to(oxform.origin);
					if (d < obj->distance * obj->distance) {
						arrived(obj, cnode);
					}
				}
#endif
			}
		}
		break;
	case NOTIFICATION_READY:
		set_physics_process(true);
		break;
	}
}