SkillManagerWrapper.cpp

#include "SkillManagerWrapper.h"

#include <mutex>

#include "RobotAPI/libraries/skills/core/SkillExecutionRequest.h"

namespace armarx::skills::gui
{
    using StatusMap = std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>;
    using SkillMap =
        std::map<skills::ProviderID, std::map<skills::SkillID, skills::SkillDescription>>;

    StatusMap
    SkillManagerWrapper::fetchExecutionsFromMemory()
    {
        if (!memory)
        {
            // check if null
            return {};
        }

        try
        {
            std::scoped_lock l(mutex_memory);

            // we return this map
            StatusMap statusMap;

            auto currentManagerStatuses = memory->ice_invocationTimeout(10000)->getSkillExecutionStatuses();

            // iterate over raw data and convert to common types
            for (const auto& [k, v] : currentManagerStatuses)
            {
                auto executionId = skills::SkillExecutionID::FromIce(k);
                auto statusUpdate = skills::SkillStatusUpdate::FromIce(v);

                // update maps
                statusMap[executionId] = statusUpdate;
            }
            return statusMap;
        }
        catch (Ice::Exception const& e)
        {
            ARMARX_WARNING
                << "Unhandled Ice exception encountered while updating executions. Exception was: "
                << e;
            emit connectionUpdate("Could not fetch executions", e.what());
            emit disableAutoUpdate();
            return {};
        }
        catch (...)
        {
            ARMARX_WARNING << "Unknown exception encountered while updating executions.";
            return {};
        }
    }

    // check if search strings occur in the skill name in order
    bool
    matches(std::string skillName, std::vector<std::string>& searches)
    {
        size_t index = 0;
        for (std::string& substring : searches)
        {
            size_t occurance = skillName.find(substring, index);
            if (occurance == std::string::npos)
            {
                return false;
            }


            // we found an occurance
            index = occurance;
        }
        return true;
    }

    SkillManagerWrapper::Snapshot
    SkillManagerWrapper::filterUpdate(Snapshot update)
    {
        // empty search => do not filter
        if (this->currentSkillSearch.empty())
        {
            return update;
        }

        Snapshot filtered;

        // for now we don't change the executions
        filtered.statuses = update.statuses;

        std::vector<std::string> substrings;

        {
            std::vector<std::string> rawSubstrings = simox::alg::split(currentSkillSearch);
            for (auto& string : rawSubstrings)
            {
                substrings.push_back(simox::alg::to_lower(string));
            }
        }


        for (auto& provider_and_descrMap : update.skills)
        {
            using DescriptionMap = std::map<skills::SkillID, skills::SkillDescription>;

            DescriptionMap& descriptionMap = provider_and_descrMap.second;
            skills::ProviderID provider = provider_and_descrMap.first;

            for (auto& skill_and_description : descriptionMap)
            {
                skills::SkillID sid = skill_and_description.first;
                skills::SkillDescription descr = skill_and_description.second;

                if (matches(simox::alg::to_lower(sid.skillName), substrings))
                {
                    // add to map
                    filtered.skills[provider][sid] = descr;
                }
            }
        }

        return filtered;
    }

    SkillMap
    SkillManagerWrapper::fetchSkillsFromMemory()
    {
        if (!memory)
        {
            return {};
        }

        try
        {
            std::scoped_lock l(mutex_memory);

            SkillMap skills;

            auto managerSkills = memory->ice_invocationTimeout(5000)->getSkillDescriptions();

            for (const auto& [sid, desc] : managerSkills)
            {
                auto description = skills::SkillDescription::FromIce(desc);
                auto skillId = skills::SkillID::FromIce(sid);
                auto providerId = skillId.providerId.value_or(
                    skills::ProviderID{.providerName = "UNKNOWN PROVIDER NAME"});

                ARMARX_CHECK(skillId.isFullySpecified());

                auto& providedSkillsMap = skills[providerId]; // create new if not existing
                providedSkillsMap.insert({skillId, description});
            }

            return skills;
        }
        catch (Ice::Exception const& e)
        {
            ARMARX_WARNING
                << "Unhandled Ice exception encountered while updating skills. Exception was: "
                << e;
            emit connectionUpdate("Could not fetch skills", e.what());
            emit disableAutoUpdate();
            return {};
        }
        catch (...)
        {
            ARMARX_WARNING << "Unknown exception encountered while updating skills.";
            return {};
        }
    }

    void
    SkillManagerWrapper::connectMemory(
        skills::manager::dti::SkillManagerInterfacePrx const& updatedMemory)
    {
        std::scoped_lock l(mutex_memory);
        this->memory = updatedMemory;
    }

    void
    SkillManagerWrapper::disconnectMemory()
    {
        // clear memory

        std::scoped_lock l(mutex_memory);
        this->memory = nullptr;
    }

    void
    SkillManagerWrapper::acceptSearchRequest(std::string const& search)
    {
        this->currentSkillSearch = search;
        emit searchAccepted();
    }

    void
    SkillManagerWrapper::stopAllExecutions()
    {
        ARMARX_IMPORTANT << "Stopping all running executions.";

        StatusMap executions;

        // we ALWAYS want the newest information when stopping all!
        // e.g. there is some new skill not known to the GUI which we explicitely want to stop too.
        // the stop-all function is often used in an emergency, so we'll live with the extra call...
        try
        {
            executions = this->fetchExecutionsFromMemory();
        }
        catch (...) // if any error occurs, we use the snapshot as backup. better to miss a skill
            // than to not do anything.
        {
            executions = this->getExecutions();
        }

        for (auto& [executionId, status] : executions)
        {
            // select all running executions...
            if (!status.hasBeenTerminated())
            {
                // ... and kill them.
                this->stopExecution(executionId, 3);
            }
        }
    }

    void
    SkillManagerWrapper::updateFromMemory()
    {
        std::scoped_lock l(mutex_snapshot);

        snapshot.skills = fetchSkillsFromMemory();
        snapshot.statuses = fetchExecutionsFromMemory();

        // notify registered widgets of update
        emit updateAvailable(filterUpdate(snapshot));
    }

    const std::optional<ProviderID>
    SkillManagerWrapper::findFirstProvider(SkillMap const& map, SkillID const& skillId)
    {
        // check if id already contains a provider. If so, this function should not have been called!
        if (skillId.isProviderSpecified())
        {
            ARMARX_WARNING << "The memory snapshot was searched for any provider, when a provider "
                              "was specified.";
            // we continue, but this might result in unexpected behaviour...
        }

        for (auto& [prov, skillMap] : map)
        {
            for (auto& [skill, desc] : skillMap)
            {
                if (skill == skillId)
                {
                    return prov;
                }
            }
        }
        return std::nullopt;
    }

    SkillMap
    SkillManagerWrapper::getSkills()
    {
        std::scoped_lock l(mutex_snapshot);

        Snapshot filtered = filterUpdate(snapshot);

        return filtered.skills;
    }

    StatusMap
    SkillManagerWrapper::getExecutions()
    {
        std::scoped_lock l(mutex_snapshot);
        return snapshot.statuses;
    }

    void
    SkillManagerWrapper::stopExecution(skills::SkillExecutionID const& executionId,
                                       const unsigned int max_retries)
    {
        // memory???
        if (!memory)
        {
            return;
        }

        unsigned int retries = max_retries;
        bool retry = false;

        do
        {
            try
            {
                ARMARX_INFO << "Aborting skill '" << executionId.skillId.skillName << "'...";
                std::scoped_lock l(mutex_memory);
                this->memory->ice_invocationTimeout(5000)->abortSkillAsync(executionId.toManagerIce());
            }
            catch (Ice::Exception const& e)
            {
                retry = true;
                ARMARX_ERROR << "Unhandeled Ice exception while aborting skill '"
                             << executionId.skillId.skillName << "'.";
                emit connectionUpdate("Could not abort skill " + executionId.skillId.skillName,
                                      e.what());
            }
            catch (...)
            {
                retry = true;
                ARMARX_ERROR << "Unhandled error while aborting skill '"
                             << executionId.skillId.skillName << "'.";
            }

            if (retry)
            {
                retries -= 1;

                if (retries > 0)
                {
                    ARMARX_WARNING << "There where errors aborting skills.  Retrying...";
                }
                else
                {
                    ARMARX_ERROR << "Couldn't abort all skills after " << max_retries
                                 << " tries.  Giving up.";
                    retry = false;
                }
            }
        } while (retry);
    }

    void
    SkillManagerWrapper::startExecutionWithParams(skills::SkillID& skillId,
                                                  aron::data::DictPtr const params)
    {
        // Memory???
        if (!memory)
        {
            return;
        }

        auto providerId = skillId.providerId;
        if (!providerId.has_value())
        {
            ARMARX_IMPORTANT << "The skill: '" << skillId.skillName
                             << "' has been requested to be executed, but no provider was "
                                "given. Aborting...";
            return;
        }

        std::map<skills::SkillID, skills::SkillDescription> skillDescriptions;
        if (this->UPDATE_ON_EXECUTION_REQUEST)
        {
            skillDescriptions = this->fetchSkillsFromMemory().at(providerId.value());
        }
        else
        {
            skillDescriptions = this->getSkills().at(providerId.value());
        }

        if (skillDescriptions.find(skillId) == skillDescriptions.end())
        {
            ARMARX_IMPORTANT << "The Skill: '" << skillId.skillName
                             << "' has been requested to be executed, but no skill description was "
                                "found. Aborting...";
            return;
        }

        char hostname[HOST_NAME_MAX];
        gethostname(hostname, HOST_NAME_MAX);

        skills::SkillExecutionRequest req{
            .skillId = skillId,
            .executorName = "Skills.Manager GUI (hostname: " + std::string(hostname) + ")",
            .parameters = params};

        ARMARX_CHECK(skillId.isFullySpecified()); // sanity check
        ARMARX_IMPORTANT << "Executing skill from GUI: " << skillId << ".";

        try
        {
            std::scoped_lock l(mutex_memory);
            memory->ice_invocationTimeout(5000)->executeSkillAsync(req.toManagerIce());
        }
        catch (Ice::Exception const& e)
        {
            ARMARX_ERROR << "Unhandeled Ice exception while executing skill '" << skillId.skillName
                         << "'. Aborting...";
            emit connectionUpdate("Execution failed of skill " + skillId.skillName, e.what());
        }
        catch (...)
        {
            ARMARX_ERROR << "Unhandled error while executing skill '" << skillId.skillName
                         << "'. Aborting...";
        }
    }


} // namespace armarx::skills::gui