#include <nanoflann.hpp>
#include <vector>
#include <iostream>
#include <flecs.h>
#include <nlohmann/json.hpp>
#include <lua.hpp>
#include "Components.h"
#include "GameData.h"
#include "CharacterManagerModule.h"
#include "CharacterModule.h"
#include "items.h"
#include "GUIModule.h"
#include "GUIModuleCommon.h"
#include "LuaData.h"
#include "PhysicsModule.h"
#include "PlayerActionModule.h"

namespace ECS
{
struct OgreVector3Adaptor {
    const std::vector<ActionNodeList::ActionNode> &nodes;

    OgreVector3Adaptor(const std::vector<ActionNodeList::ActionNode> &nodes)
        : nodes(nodes)
    {
    }

    // Required by nanoflann: Number of data points
    inline size_t kdtree_get_point_count() const
    {
        return nodes.size();
    }

    // Required by nanoflann: Returns the distance between the vector and a point
    // Using squared distance is standard for performance
    inline float kdtree_get_pt(const size_t idx, const size_t dim) const
    {
        return nodes[idx].position[dim];
    }

    // Optional: bounding box optimization (return false if not implemented)
    template <class BBOX> bool kdtree_get_bbox(BBOX & /*bb*/) const
    {
        return false;
    }
};

typedef nanoflann::KDTreeSingleIndexAdaptor<
    nanoflann::L2_Simple_Adaptor<float, OgreVector3Adaptor>,
    OgreVector3Adaptor, 3 /* dimensionality */
    >
    OgreKDTree;
struct ActionNodeList::indexObject {
    OgreVector3Adaptor adaptor;
    OgreKDTree index;
    indexObject(const std::vector<ActionNodeList::ActionNode> &nodes)
        : adaptor(nodes)
        , index(3, adaptor,
            nanoflann::KDTreeSingleIndexAdaptorParams(10))
    {
    }
};

struct TestNarrativeHandler : GUI::NarrationHandler {
    int count;
    TestNarrativeHandler()
        : GUI::NarrationHandler()
        , count(0)
    {
    }
    void finish() override
    {
        _clear_narration();
    }
    void activate() override
    {
        _narration("Greetings...", {});
        std::cout << getPropsJSON().dump(4) << std::endl;
        count = 0;
    }
    void event(const Ogre::String &evt) override
    {
        if (evt == "narration_progress" ||
            evt == "narration_answered") {
            count++;
            if (count == 1) {
                _narration(
                    "Question..." +
                        Ogre::StringConverter::toString(
                            count),
                    { "Answer1", "Answer2" });
            } else {
                _narration(
                    "Whatever..." +
                        Ogre::StringConverter::toString(
                            count),
                    {});
            }
            if (count > 5)
                _finish();
        }
        if (evt == "narration_answered")
            std::cout << "answer: " << getNarrationAnswer()
                  << std::endl;
    }
};
struct LuaNarrationHandler : GUI::NarrationHandler {
    int ref;
    lua_State *L;
    LuaNarrationHandler(lua_State *L, int ref)
        : ref(ref)
        , L(L)
    {
        lua_rawgeti(L, LUA_REGISTRYINDEX, ref);
        lua_pushlightuserdata(L, this);
        lua_pushcclosure(
            L,
            [](lua_State *L) {
                luaL_checktype(L, 1, LUA_TSTRING);
                luaL_checktype(L, 2, LUA_TTABLE);
                LuaNarrationHandler *handler =
                    static_cast<LuaNarrationHandler *>(
                        lua_touserdata(
                            L,
                            lua_upvalueindex(1)));
                Ogre::String event = lua_tostring(L, 1);
                std::vector<Ogre::String> choices;
                int choicesLen = (int)lua_rawlen(L, 2);
                choices.reserve(choicesLen);
                for (int i = 1; i <= choicesLen; ++i) {
                    lua_rawgeti(L, 2, i);
                    if (lua_isstring(L, -1))
                        choices.push_back(
                            lua_tostring(L, -1));
                    lua_pop(L, 1);
                }
                handler->_narration(event, choices);
                return 0;
            },
            1);
        lua_setfield(L, -2, "_narration");
        lua_pushlightuserdata(L, this);
        lua_pushcclosure(
            L,
            [](lua_State *L) {
                LuaNarrationHandler *handler =
                    static_cast<LuaNarrationHandler *>(
                        lua_touserdata(
                            L,
                            lua_upvalueindex(1)));
                handler->_finish();
                return 0;
            },
            1);
        lua_setfield(L, -2, "_finish");
        lua_pushlightuserdata(L, this);
        lua_pushcclosure(
            L,
            [](lua_State *L) {
                LuaNarrationHandler *handler =
                    static_cast<LuaNarrationHandler *>(
                        lua_touserdata(
                            L,
                            lua_upvalueindex(1)));
                int answer = handler->getNarrationAnswer();
                lua_pushinteger(L, answer);
                return 1;
            },
            1);
        lua_setfield(L, -2, "_get_narration_answer");
        lua_pushlightuserdata(L, this);
        lua_pushcclosure(
            L,
            [](lua_State *L) {
                LuaNarrationHandler *handler =
                    static_cast<LuaNarrationHandler *>(
                        lua_touserdata(
                            L,
                            lua_upvalueindex(1)));
                lua_pushstring(
                    L, handler->getProperties().c_str());
                return 1;
            },
            1);
        lua_setfield(L, -2, "_get_properties");
        lua_pushlightuserdata(L, this);
        lua_pushcclosure(
            L,
            [](lua_State *L) {
                LuaNarrationHandler *handler =
                    static_cast<LuaNarrationHandler *>(
                        lua_touserdata(
                            L,
                            lua_upvalueindex(1)));
                lua_pushstring(
                    L, handler->getProperties().c_str());
                const std::vector<ActionNodeList::ActionNode>
                    &nodes = ECS::get<ActionNodeList>()
                             .dynamicNodes;
                lua_newtable(L);
                int i;
                for (i = 0; i < nodes.size(); i++) {
                    lua_pushinteger(L, i + 1);
                    lua_pushstring(
                        L,
                        nodes[i].props.dump().c_str());
                    lua_settable(L, -3);
                }
                return 1;
            },
            1);
        lua_setfield(L, -2, "_get_goals");
        lua_pop(L, 1);
    }
    void finish() override
    {
        lua_rawgeti(L, LUA_REGISTRYINDEX, ref);
        int type = lua_getfield(L, -1, "finish");
        OgreAssert(type == LUA_TFUNCTION, "bad finish()");
        lua_insert(L, -2);
        if (lua_pcall(L, 1, 0, 0) != 0) {
            std::cerr << lua_tostring(L, -1) << std::endl;
            OgreAssert(false, "lua error");
            lua_pop(L, 1);
        }
        _clear_narration();
    }
    void activate() override
    {
        lua_rawgeti(L, LUA_REGISTRYINDEX, ref);
        int type = lua_getfield(L, -1, "activate");
        OgreAssert(type == LUA_TFUNCTION, "bad activate()");
        lua_insert(L, -2);
        if (lua_pcall(L, 1, 0, 0) != 0) {
            std::cerr << lua_tostring(L, -1) << std::endl;
            OgreAssert(false, "lua error");
            lua_pop(L, 1);
        }
    }
    void event(const Ogre::String &evt) override
    {
        lua_rawgeti(L, LUA_REGISTRYINDEX, ref);
        int type = lua_getfield(L, -1, "event");
        OgreAssert(type == LUA_TFUNCTION, "bad event()");
        lua_insert(L, -2);
        lua_pushstring(L, evt.c_str());
        if (lua_pcall(L, 2, 0, 0) != 0) {
            std::cerr << lua_tostring(L, -1) << std::endl;
            OgreAssert(false, "lua error");
            lua_pop(L, 1);
        }
    }
};

struct SimpleWordHandler : PlayerActionModule::ActionWordHandler {
    void operator()(int actor, flecs::entity town, int index,
            const Ogre::String &word, int actionNode) override
    {
        if (index >= 0) {
            TestNarrativeHandler *handle =
                OGRE_NEW TestNarrativeHandler();
            /* this is for NPCs only, right? */
            const TownNPCs::NPCData &npc =
                town.get<TownNPCs>().npcs.at(index);
            flecs::entity e = npc.e;
            for (const auto &anode : npc.actionNodes) {
                if (anode.action == word) {
                    nlohmann::json props = anode.props;
                    props["initiator"] = actor;
                    props["recipient"] = index;
                    handle->setProperties(props.dump());
                    break;
                }
            }
            ECS::get_mut<GUI>().addNarrationHandler(handle);
            ECS::modified<GUI>();
        }
    }
};

PlayerActionModule::PlayerActionModule(flecs::world &ecs)
{
    ecs.module<PlayerActionModule>();
    ecs.import <CharacterManagerModule>();
    ecs.component<ActionNodeList>()
        .on_add([](flecs::entity e, ActionNodeList &alist) {
            alist.nodeMutex = std::make_shared<std::mutex>();
            alist.setDirty();
            alist.nodes.reserve(1000);
            alist.dynamicNodes.reserve(1000);
            alist.setUISelected(-1);
            alist.setReady();
        })
        .add(flecs::Singleton);
    ecs.system<ActionNodeList>("updateNodeList")
        .kind(flecs::OnUpdate)
        .each([](ActionNodeList &list) {
            if (list.isBusy())
                return;
            if (list.nodes.size() > 0) {
                Ogre::SceneNode *cameraNode =
                    ECS::get<Camera>().mCameraNode;
                Ogre::Vector3 cameraPos =
                    cameraNode->_getDerivedPosition();
                flecs::entity player =
                    ECS::get<CharacterManagerModule>()
                        .getPlayer();
                if (player.is_valid()) {
                    Ogre::Vector3 playerPos =
                        player.get<CharacterBase>()
                            .mBodyNode
                            ->_getDerivedPosition();
                    list.UIquery(playerPos);
                } else {
                    list.UIquery(cameraPos);
                }
            }
        });
    ecs.system<ActionNodeList, const Input>("ActivateActionNode")
        .kind(flecs::OnUpdate)
        .each([this](ActionNodeList &list, const Input &input) {
            std::lock_guard<std::mutex> lock(*list.nodeMutex);
            if (input.control & 32)
                std::cout << "act pressed" << std::endl;
            if (list.isBusy())
                return;
            if (input.act_pressed &&
                list.getUIData().selected >= 0) {
                std::cout
                    << list.dynamicNodes[list.getUIData()
                                     .selected]
                           .props.dump(4)
                    << std::endl;
                flecs::entity_t townid =
                    list.dynamicNodes[list.getUIData()
                                  .selected]
                        .props["town"]
                        .get<flecs::entity_t>();
                flecs::entity town = ECS::get().entity(townid);
                int index = list.dynamicNodes[list.getUIData()
                                      .selected]
                            .props["index"]
                            .get<int>();
                for (auto it = actionWords.begin();
                     it != actionWords.end(); it++) {
                    if (it->first ==
                        list.dynamicNodes[list.getUIData()
                                      .selected]
                            .action) {
                        (*it->second)(
                            -1, town, index,
                            list.dynamicNodes
                                [list.getUIData()
                                     .selected]
                                    .action,
                            list.getUIData()
                                .selected);
                        list.setBusy();
                    }
                }
            }
            if (!ECS::get<GUI>().enabled)
                list.setReady();
        });
    ecs.system<const EngineData>("UpdateActivatedWords")
        .kind(flecs::OnUpdate)
        .each([this](const EngineData &eng) {
            for (auto it = activatedWords.begin();
                 it != activatedWords.end();) {
                int ret = it->second->_update(eng.delta);
                if (ret != ActivatedWordHandler::BUSY) {
                    delete it->second;
                    it = activatedWords.erase(it);
                } else
                    it++;
            }
        });
}

void PlayerActionModule::addWordHandler(const Ogre::String &word,
                    ActionWordHandler *handler)
{
    actionWords.insert({ word, handler });
}

void PlayerActionModule::removeWordHandler(const Ogre::String &word,
                       ActionWordHandler *handler)
{
    for (auto it = actionWords.begin(); it != actionWords.end();) {
        if (it->first == word && it->second == handler)
            it = actionWords.erase(it);
        else
            it++;
    }
}

static std::set<int> activeActors;
struct TestActivatedWordHandler : PlayerActionModule::ActivatedWordHandler {
    int actor;
    flecs::entity town;
    int index;
    Ogre::String word;
    int actionNode;
    ActionNodeList::ActionNode anode;
    flecs::entity ch;
    int state;
    float delay;
    std::unordered_map<Ogre::String, Ogre::Vector3> placeLocalOffset;
    std::unordered_map<Ogre::String, Ogre::Quaternion> placeLocalRotation;
    TestActivatedWordHandler(int actor, flecs::entity town, int index,
                 const Ogre::String &word, int actionNode)
        : PlayerActionModule::ActivatedWordHandler()
        , actor(actor)
        , town(town)
        , index(index)
        , word(word)
        , actionNode(actionNode)
        , state(0)
        , delay(0)
    {
        activeActors.insert(actor);
        // dynamic nodes can disappear on us so to avoid that use a copy
        anode = ECS::get<ActionNodeList>().dynamicNodes[actionNode];
        if (actor == -1)
            ch = ECS::get<CharacterManagerModule>().getPlayer();
        else if (actor >= 0) {
            const TownNPCs &npcs = town.get<TownNPCs>();
            ch = npcs.npcs.at(actor).e;
        }
        if (anode.props.find("positions") == anode.props.end())
            goto out;
        for (const auto &position : anode.props["positions"]) {
            if (position.find("name") == position.end())
                continue;
            Ogre::Vector3 localPosition;
            Ogre::Quaternion localRotation;
            localPosition.x = position["position_x"].get<float>();
            localPosition.y = position["position_y"].get<float>();
            localPosition.z = position["position_z"].get<float>();
            localRotation.w = position["rotation_w"].get<float>();
            localRotation.x = position["rotation_x"].get<float>();
            localRotation.y = position["rotation_y"].get<float>();
            localRotation.z = position["rotation_z"].get<float>();
            placeLocalOffset[position["name"].get<Ogre::String>()] =
                localPosition;
            placeLocalRotation[position["name"].get<Ogre::String>()] =
                localRotation;
        }
out:;
    }
    void teleport(const Ogre::String &place)
    {
        if (placeLocalOffset.find(place) == placeLocalOffset.end())
            return;
        std::cout << "local offset: " << placeLocalOffset[place]
              << std::endl;
        std::cout << "parent offset: " << anode.position << std::endl;
        Ogre::Quaternion newRotation =
            anode.rotation * placeLocalRotation[place];
        Ogre::Vector3 newPosition =
            anode.position +
            anode.rotation * placeLocalOffset[place];
        if (ch.is_valid() && ch.has<CharacterBase>()) {
            ch.get<CharacterBase>()
                .mBodyNode->_setDerivedOrientation(newRotation);
            ch.get<CharacterBase>().mBodyNode->_setDerivedPosition(
                newPosition);
        }
        if (actor >= 0) {
            town.get_mut<TownNPCs>().npcs[actor].position =
                newPosition;
            town.get_mut<TownNPCs>().npcs[actor].orientation =
                newRotation;
            town.modified<TownNPCs>();
        }
    }
    int update(float delta)
    {
        switch (state) {
        case 0:
            if (ECS::get<Input>().act)
                delay += delta;
            // activate anly after delay
            if (ECS::get<Input>().act == 0 && delay > 0.2f) {
                delay = 0.0f;
                state = 10;
                // Yay!!!
                std::cout << "Node data: " << std::endl;
                std::cout << anode.props.dump(4) << std::endl;
            } else if (ECS::get<Input>().act == 0 &&
                   delay <= 0.2f) {
                delay = 0.0f;
                state = 100;
            }
            break;
        case 10:
            if (ch.is_valid()) {
                PhysicsModule::controlPhysics(ch, false);
                // no control by player or ai
                ch.add<CharacterControlDisable>();
                //            else
                //                ch.set<CharacterInActuator>(
                //                    { "idle", { 0, 0, 0 } });
            }
            delay = 0.0f;
            state++;
            break;
        case 11:
            teleport("enter");
            delay = 0.0f;
            state++;
            break;
        case 12:
            if (ch.is_valid()) {
                if (word == "sit")
                    ch.set<CharacterInActuator>(
                        { "sitting-chair",
                          { 0, 0, 0 } });
            }
            teleport("enter");
            delay = 0.0f;
            state++;
            break;
        case 13:
            teleport("enter");
            delay = 0.0f;
            state = 50;
            break;
        case 50:
            // teleport again to handle possible problems caused by root motion
            delay = 0.0f;
            teleport("enter");
            state++;
            break;
        case 51:
            // do not move anywhere until we depress key and wait a bit
            if (ECS::get<Input>().act == 0)
                delay += delta;
            if (delay > 1.0f) {
                delay = 0.0f;
                state++;
            }
            break;
        case 52:
            // if the key is pressed for a second move to next state
            if (ECS::get<Input>().act)
                delay += delta;
            if (delay > 1.0f) {
                delay = 0.0f;
                state++;
            }
            break;
        case 53:
            delay = 0.0;
            state = 100;
            break;
        case 100:
            delay = 0.0;
            state++;
            break;
        case 101:
            // wait until key is depressed for a second
            if (ECS::get<Input>().act == 0)
                delay += delta;
            if (delay > 1.0f) {
                delay = 0.0;
                state++;
            }
            break;
        case 102:
            delay = 0.0f;
            state = 0;
            return OK;
            break;
        }
        std::cout << this << " delay: " << delay << " state: " << state
              << std::endl;
        return BUSY;
    }
    void enter()
    {
        delay = 0.0f;
        state = 0;
        ECS::get_mut<GUI>().enableActions = false;
        ECS::modified<GUI>();
        std::cout << "enter" << std::endl;
    }
    void exit(int result)
    {
        ch.remove<CharacterInActuator>();
        ch.remove<CharacterControlDisable>();
        PhysicsModule::controlPhysics(ch, true);
        // OgreAssert(false, "exit");
        delay = 0.0f;
        state = 0;
        ECS::get_mut<GUI>().enableActions = true;
        ECS::modified<GUI>();
        std::cout << "exit" << std::endl;
    }
    virtual ~TestActivatedWordHandler()
    {
        activeActors.erase(actor);
    }
};

struct LuaWordHandler : PlayerActionModule::ActionWordHandler {
    lua_State *L;
    int ref;
    void operator()(int actor, flecs::entity town, int index,
            const Ogre::String &word, int actionNode) override
    {
        lua_rawgeti(L, LUA_REGISTRYINDEX, ref);
        if (lua_type(L, -1) == LUA_TFUNCTION) {
            luaL_checktype(L, -1, LUA_TFUNCTION);
            lua_pushinteger(L, town.id());
            lua_pushinteger(L, index);
            lua_pushstring(L, word.c_str());
            if (lua_pcall(L, 3, 0, 0)) {
                Ogre::LogManager::getSingleton().stream()
                    << lua_tostring(L, -1);
                OgreAssert(false, "Lua error");
            }
        } else if (lua_type(L, -1) == LUA_TTABLE) {
            luaL_checktype(L, -1, LUA_TTABLE);
            lua_pop(L, 1);
            /* trying to talk to NPC activates narration mode */
            if (index >= 0 && word == "talk") {
                LuaNarrationHandler *handle =
                    OGRE_NEW LuaNarrationHandler(L, ref);
                const TownNPCs::NPCData &npc =
                    town.get<TownNPCs>().npcs.at(index);
                flecs::entity e = npc.e;
                for (const auto &anode : npc.actionNodes) {
                    if (anode.action == word) {
                        nlohmann::json props =
                            anode.props;
                        props["initiator"] = actor;
                        props["recipient"] = index;
                        handle->setProperties(
                            props.dump());
                        break;
                    }
                }
                ECS::get_mut<GUI>().addNarrationHandler(handle);
                ECS::modified<GUI>();
            } else {
                if (activeActors.find(actor) ==
                    activeActors.end()) {
                    TestActivatedWordHandler *handler =
                        OGRE_NEW TestActivatedWordHandler(
                            actor, town, index,
                            word, actionNode);
                    ECS::get_mut<PlayerActionModule>()
                        .addActivatedWordHandler(
                            word, handler);
                    ECS::modified<PlayerActionModule>();
                }
            }
        }
    }
};
void PlayerActionModule::addLuaWordHandler(const Ogre::String &word,
                       lua_State *L, int ref)
{
    struct LuaWordHandler *handler = OGRE_NEW LuaWordHandler;
    handler->L = L;
    handler->ref = ref;
    addWordHandler(word, handler);
}

void PlayerActionModule::removeLuaWordHandler(const Ogre::String &word,
                          lua_State *L, int ref)
{
    for (auto it = actionWords.begin(); it != actionWords.end();) {
        LuaWordHandler *handler =
            static_cast<LuaWordHandler *>(it->second);
        if (it->first == word && handler->L == L && handler->ref == ref)
            it = actionWords.erase(it);
        else
            it++;
    }
}

int PlayerActionModule::setupLuaActionHandler(lua_State *L)
{
    luaL_checktype(L, 1, LUA_TSTRING);
    if (lua_type(L, 2) == LUA_TFUNCTION) {
        luaL_checktype(L, 2, LUA_TFUNCTION);
        Ogre::String word = lua_tostring(L, 1);
        lua_pushvalue(L, 2);
        int ref = luaL_ref(L, LUA_REGISTRYINDEX);
        addLuaWordHandler(word, L, ref);
    } else if (lua_type(L, 2) == LUA_TTABLE) {
        luaL_checktype(L, 2, LUA_TTABLE);
        Ogre::String word = lua_tostring(L, 1);
        lua_pushvalue(L, 2);
        int ref = luaL_ref(L, LUA_REGISTRYINDEX);
        addLuaWordHandler(word, L, ref);
    }
    return 0;
}

void PlayerActionModule::addActivatedWordHandler(const Ogre::String &word,
                         ActivatedWordHandler *handler)
{
    for (auto it = activatedWords.begin(); it != activatedWords.end();
         it++) {
    }
    activatedWords.insert({ word, handler });
}

void PlayerActionModule::removeActivatedWordHandler(
    const Ogre::String &word, ActivatedWordHandler *handler)
{
    for (auto it = activatedWords.begin(); it != activatedWords.end();) {
        if (it->first == word && it->second == handler)
            it = activatedWords.erase(it);
        else
            it++;
    }
}

void ActionNodeList::updateDynamicNodes()
{
    std::lock_guard<std::mutex> lock(*nodeMutex);
    if (dynamicNodes.size() > nodes.size())
        dynamicNodes.resize(nodes.size());
    else {
        dynamicNodes.clear();
        dynamicNodes.insert(dynamicNodes.end(), nodes.begin(),
                    nodes.end());
    }
    ECS::get().query_builder<const TownNPCs>().each(
        [this](flecs::entity town, const TownNPCs &npcs) {
            for (auto it = npcs.npcs.begin(); it != npcs.npcs.end();
                 it++) {
                dynamicNodes.insert(
                    dynamicNodes.end(),
                    it->second.actionNodes.begin(),
                    it->second.actionNodes.end());
            }
        });
    dirty = true;
}

void ActionNodeList::build()
{
    std::lock_guard<std::mutex> lock(*nodeMutex);
    indexObj = std::make_shared<ActionNodeList::indexObject>(dynamicNodes);
    indexObj->index.buildIndex();
    dirty = false;
}

bool ActionNodeList::_query(const Ogre::Vector3 &position,
                std::vector<size_t> &points,
                std::vector<float> &distances)
{
    std::vector<size_t> tmppoints;
    std::vector<float> tmpdistances;
    points.clear();
    points.reserve(4);
    distances.clear();
    distances.reserve(4);
    tmppoints.resize(4);
    tmpdistances.resize(4);
    if (!indexObj) {
        dirty = true;
        return false;
    }
    nanoflann::KNNResultSet<float> resultSet(4);
    resultSet.init(tmppoints.data(), tmpdistances.data());
    bool ret = indexObj->index.findNeighbors(resultSet, &position.x,
                         nanoflann::SearchParameters());
    int i;
    for (i = 0; i < resultSet.size(); i++)
        if (tmpdistances[i] < 25.0f) {
            points.push_back(tmppoints[i]);
            distances.push_back(tmpdistances[i]);
        }
    return ret;
}

bool ActionNodeList::query_ai(const Ogre::Vector3 &position, float distance,
                  std::vector<size_t> &points,
                  std::vector<float> &distances)
{
    std::lock_guard<std::mutex> lock(*nodeMutex);
    std::vector<size_t> tmppoints;
    std::vector<float> tmpdistances;
    points.clear();
    points.reserve(100);
    distances.clear();
    distances.reserve(100);
    tmppoints.resize(100);
    tmpdistances.resize(100);
    if (!indexObj) {
        dirty = true;
        return false;
    }
    nanoflann::KNNResultSet<float> resultSet(100);
    resultSet.init(tmppoints.data(), tmpdistances.data());
    bool ret = indexObj->index.findNeighbors(resultSet, &position.x,
                         nanoflann::SearchParameters());
    int i;
    for (i = 0; i < resultSet.size(); i++)
        if (tmpdistances[i] < distance) {
            points.push_back(tmppoints[i]);
            distances.push_back(tmpdistances[i]);
        }
    return ret;
}

int ActionNodeList::addNode(ActionNode &node)
{
    std::lock_guard<std::mutex> lock(*nodeMutex);
    int index = nodes.size();
    nodes.push_back(node);
    dirty = true;
    return index;
}

void ActionNodeList::removeNode(int index)
{
    std::lock_guard<std::mutex> lock(*nodeMutex);
    nodes.erase(nodes.begin() + index);
    dirty = true;
}

const ActionNodeList::UIData &ActionNodeList::getUIData()
{
    std::lock_guard<std::mutex> lock(*uidata.mutex);
    return uidata;
}

void ActionNodeList::setUISelected(int selected)
{
    std::lock_guard<std::mutex> lock(*uidata.mutex);

    uidata.selected = selected;
}

void ActionNodeList::setUIPoints(const std::vector<size_t> &points,
                 const std::vector<float> &distances)
{
    std::lock_guard<std::mutex> lock(*uidata.mutex);
    uidata.points = points;
    uidata.distances = distances;
}

void ActionNodeList::UIquery(const Ogre::Vector3 &position)
{
    bool needBuild = false;

    {
        std::lock_guard<std::mutex> lock(*nodeMutex);
        if (dirty || !indexObj)
            needBuild = true;
    }
    if (needBuild)
        build();
    {
        std::lock_guard<std::mutex> lock(*uidata.mutex);
        _query(position, uidata.points, uidata.distances);
    }
}

void ActionNodeList::setDirty()
{
    dirty = true;
}

void ActionNodeList::setReady()
{
    busy = false;
}

void ActionNodeList::setBusy()
{
    busy = true;
}

bool ActionNodeList::isBusy()
{
    return busy;
}
}