SkillManagerComponentPluginUser.cpp

#include "SkillManagerComponentPluginUser.h"

#include <mutex>
#include <optional>
#include <string>
#include <vector>
#include <boost/uuid/uuid_io.hpp>

#include <Ice/Exception.h>
#include <Ice/OutputStream.h>
#include <IceUtil/Exception.h>
#include <IceUtil/Optional.h>

#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/time.h>

#include "RobotAPI/libraries/aron/core/data/variant/container/Dict.h"
#include "RobotAPI/libraries/skills/core/FluxioProfile.h"
#include "RobotAPI/libraries/skills/core/FluxioProvider.h"
#include "RobotAPI/libraries/skills/core/FluxioSkill.h"
#include "RobotAPI/libraries/skills/core/error/FluxioErrorMessages.h"
#include "RobotAPI/libraries/skills/core/error/FluxioException.h"
#include "RobotAPI/libraries/skills/manager/SkillManagerComponentPlugin.h"
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/skills/core/aron/FluxioProfile.aron.generated.h>
#include <RobotAPI/libraries/skills/core/aron/FluxioSkill.aron.generated.h>

namespace armarx
{
    SkillManagerComponentPluginUser::SkillManagerComponentPluginUser()
    {
        addPlugin(plugin);
    }

    void
    SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
                                                 const Ice::Current&)
    {
        auto i = skills::ProviderInfo::FromIce(info);
        this->plugin->addProvider(i);
        std::string providerId = boost::uuids::to_string(armarx::plugins::SkillManagerComponentPlugin::createUuidWithString(info.providerId.providerName));
        std::optional<std::vector<skills::manager::arondto::FluxioSkill>> opt =
            loadCompositeSkillsOfProvider(providerId);

        if (!opt.has_value()) {
            ARMARX_ERROR << "Failed to load composite skills for provider " << info.providerId.providerName;
            return;
        }

        for (const auto& skill : opt.value())
        {
            // TODO: Implement a proper way to load all skills from memory without breaking dependencies etc.
        }
    }

    void
    SkillManagerComponentPluginUser::removeProvider(
        const skills::manager::dto::ProviderID& provider,
        const Ice::Current&)
    {
        auto i = skills::ProviderID::FromIce(provider);
        this->plugin->removeProvider(i);
    }

    skills::manager::dto::SkillStatusUpdate
    SkillManagerComponentPluginUser::executeSkill(
        const skills::manager::dto::SkillExecutionRequest& info,
        const Ice::Current&)
    {
        auto e = skills::SkillExecutionRequest::FromIce(info);
        return this->plugin->executeSkill(e).toManagerIce();
    }

    skills::manager::dto::SkillExecutionID
    SkillManagerComponentPluginUser::executeSkillAsync(
        const skills::manager::dto::SkillExecutionRequest& info,
        const Ice::Current& current)
    {
        auto e = skills::SkillExecutionRequest::FromIce(info);
        return this->plugin->executeSkillAsync(e).toManagerIce();
    }

    skills::provider::dto::ParameterUpdateResult
    SkillManagerComponentPluginUser::updateSkillParameters(
        const skills::manager::dto::SkillExecutionID& info,
        const aron::data::dto::DictPtr& params,
        const Ice::Current& current)
    {
        skills::provider::dto::ParameterUpdateResult ret;
        auto a = armarx::aron::data::Dict::FromAronDictDTO(params);
        auto e = skills::SkillExecutionID::FromIce(info);
        ret.success = this->plugin->updateSkillParameters(e, a);
        return ret;
    }

    skills::provider::dto::AbortSkillResult
    SkillManagerComponentPluginUser::abortSkill(const skills::manager::dto::SkillExecutionID& id,
                                                const Ice::Current& current)
    {
        skills::provider::dto::AbortSkillResult ret;
        auto i = skills::SkillExecutionID::FromIce(id);
        ret.success = this->plugin->abortSkill(i);
        return ret;
    }

    skills::provider::dto::AbortSkillResult
    SkillManagerComponentPluginUser::abortSkillAsync(
        const skills::manager::dto::SkillExecutionID& id,
        const Ice::Current& /*unused*/)
    {
        skills::provider::dto::AbortSkillResult ret;
        auto i = skills::SkillExecutionID::FromIce(id);
        ret.success = this->plugin->abortSkillAsync(i);
        return ret;
    }

    void
    SkillManagerComponentPluginUser::updateStatusForSkill(
        const skills::provider::dto::SkillStatusUpdate& statusUpdate,
        const skills::callback::dto::ProviderID& pid,
        const Ice::Current&)
    {
        (void)statusUpdate;
        (void)pid;
        // If you want to use the status, implement this method!
    }

    skills::manager::dto::SkillDescriptionMap
    SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
    {
        skills::manager::dto::SkillDescriptionMap ret;

        auto m = this->plugin->getSkillDescriptions();

        for (const auto& [k, v] : m)
        {
            ret.insert({k.toManagerIce(), v.toManagerIce()});
        }

        return ret;
    }

    IceUtil::Optional<skills::manager::dto::SkillDescription>
    SkillManagerComponentPluginUser::getSkillDescription(const skills::manager::dto::SkillID& id,
                                                         const Ice::Current& current)
    {
        auto e = skills::SkillID::FromIce(id);
        auto o = this->plugin->getSkillDescription(e);
        if (o.has_value())
        {
            return o->toManagerIce();
        }
        return {};
    }

    IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
    SkillManagerComponentPluginUser::getSkillExecutionStatus(
        const skills::manager::dto::SkillExecutionID& executionId,
        const Ice::Current& current)
    {
        auto e = skills::SkillExecutionID::FromIce(executionId);
        auto o = this->plugin->getSkillExecutionStatus(e);
        if (o.has_value())
        {
            return o->toManagerIce();
        }
        return {};
    }

    skills::manager::dto::SkillStatusUpdateMap
    SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
    {
        skills::manager::dto::SkillStatusUpdateMap ret;

        auto m = this->plugin->getSkillExecutionStatuses();

        for (const auto& [k, v] : m)
        {
            ret.insert({k.toManagerIce(), v.toManagerIce()});
        }

        return ret;
    }

    //****************************//
    //** Fluxio related methods **//
    //****************************//

    aron::type::dto::AronObjectPtr
    SkillManagerComponentPluginUser::getTypes(const Ice::Current& current)
    {
        const auto& res = this->plugin->getTypes();
        return res->toAronObjectDTO();
    }

    aron::data::dto::DictPtr
    SkillManagerComponentPluginUser::getTestData(const Ice::Current& current)
    {
        aron::data::DictPtr res = make_shared<aron::data::Dict>();
        const auto& execs = this->plugin->fluxioDC.fluxioExecutors;
        for (const auto& [k, v] : execs)
        {
            auto data = v->getResultsCopy();
            if (data != nullptr)
            {
                for (const auto& entry : data->getElements())
                {
                    const auto key = k + " (" + entry.first + ")";
                    res->addElement(key, entry.second);
                }
            }
        }
        return res->toAronDictDTO();
    }

    IceUtil::Optional<std::string>
    SkillManagerComponentPluginUser::executeFluxioSkill(const std::string& skillId,
                                                        const std::string& profileId,
                                                        const Ice::Current& current)
    {
        const auto& res = this->plugin->executeFluxioSkill(skillId, profileId, "Fluxio");
        if (!res.isSuccess())
        {
            auto e = res.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }
        if (res.getResult() == nullptr)
        {
            auto e = res.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }

        return res.getResult()->id;
    }

    void
    SkillManagerComponentPluginUser::abortFluxioSkill(const std::string& executionId,
                                                      const Ice::Current& current)
    {
        this->plugin->abortFluxioSkill(executionId);
    }

    IceUtil::Optional<skills::manager::dto::FluxioSkillStatusUpdateList>
    SkillManagerComponentPluginUser::getFluxioSkillExecutionStatus(const std::string& executionId,
                                                                   const Ice::Current& current)
    {
        auto l = this->plugin->getFluxioSkillExecutionStatus(executionId);
        if (!l.isSuccess())
        {
            ARMARX_WARNING << "Error getting FluxioSkillExecutionStatus";

            auto e = l.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }

        skills::manager::dto::FluxioSkillStatusUpdateList ret;

        for (const auto& s : l.getResult())
        {
            ret.push_back(s.toManagerIce());
        }

        return ret;
    }

    skills::manager::dto::FluxioSkillList
    SkillManagerComponentPluginUser::getSkillList(const Ice::Current& current)
    {
        skills::manager::dto::FluxioSkillList ret;

        auto l = this->plugin->getSkillList();

        if (!l.isSuccess())
        {
            auto e = l.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }

        for (const auto& s : l.getResult())
        {
            if (s == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr!";
                continue;
            }

            const auto& skill = s->toManagerIce();

            if (!skill.has_value())
            {
                ARMARX_WARNING << "Skill with id " << s->id << " could not be converted";
                continue;
            }

            ret.push_back(skill.value());
        }

        return ret;
    }

    IceUtil::Optional<skills::manager::dto::FluxioSkill>
    SkillManagerComponentPluginUser::getSkill(const std::string& id, const Ice::Current& current)
    {
        auto result = this->plugin->getSkill(id);

        if (!result.isSuccess())
        {
            auto e = result.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }
        const auto& skill = result.getResult();
        if (skill == nullptr)
        {
            return {};
        }

        const auto& s = skill->toManagerIce();

        if (!s.has_value())
        {
            return {};
        }

        return s.value();
    }

    bool
    SkillManagerComponentPluginUser::updateSkill(const std::string& userId,
                                                 const skills::manager::dto::FluxioSkill& skill,
                                                 const Ice::Current& current)
    {
        std::scoped_lock l(this->plugin->fluxioDC.skillsMutex,
                           this->plugin->fluxioDC.profilesMutex,
                           this->plugin->fluxioDC.providersMutex,
                           this->plugin->fluxioDC.typesMutex);
        auto& skillsMap = this->plugin->fluxioDC.skills;
        auto& providersMap = this->plugin->fluxioDC.providers;
        auto& profilesMap = this->plugin->fluxioDC.profiles;
        auto& typesMap = this->plugin->fluxioDC.types;
        const auto& s = skillsMap.find(skill.id);

        // Check if the skill exists
        if (s == skillsMap.end())
        {
            ARMARX_WARNING << "Skill with id " << skill.id << " not found";
            return false;
        }

        // Check if the user has the mutex for the skill
        auto res = this->plugin->getSkillMutex(skill.id, userId);
        if (!res.isSuccess())
        {
            ARMARX_WARNING << "User " << userId << "User does not have Mutex for this Skill"
                           << skill.id;

            auto error = res.getError();
            error.addToContext(skills::error::createErrorMessage(
                                   skills::error::ErrorCode::UserHasNoMutexError, {skill.id}),
                               "SkillManagerComponentPluginUser",
                               __FUNCTION__,
                               __LINE__);
            throw error.toManagerIce();
        }

        const bool ret =
            s->second.updateFromIce(skill, providersMap, profilesMap, skillsMap, typesMap);

        std::optional<skills::manager::arondto::FluxioSkill> opt = s->second.toAronXml();

        if (!opt.has_value())
        {
            ARMARX_WARNING << "Skill with id " << skill.id << " could not be converted";
            return false;
        }

        saveSkill(opt.value());

        return ret;
    }

    bool
    SkillManagerComponentPluginUser::updateSkillParameterValues(
        const std::string& userId,
        const std::string& skillId,
        const std::string& parameterId,
        const skills::manager::dto::FluxioValueList& values,
        const Ice::Current& current)
    {
        std::scoped_lock l(this->plugin->fluxioDC.skillsMutex,
                           this->plugin->fluxioDC.profilesMutex);
        auto& skillsMap = this->plugin->fluxioDC.skills;
        auto& profilesMap = this->plugin->fluxioDC.profiles;

        // check if skill exists
        const auto& skill = skillsMap.find(skillId);

        if (skill == skillsMap.end())
        {
            ARMARX_WARNING << "Skill with id " << skillId << " not found";
            return false;
        }

        // Check if the user has the mutex for the skill
        auto res = this->plugin->getSkillMutex(skillId, userId);
        if (!res.isSuccess())
        {
            ARMARX_WARNING << "User " << userId << "User does not have Mutex for this Skill"
                           << skillId;

            auto error = res.getError();
            error.addToContext(skills::error::createErrorMessage(
                                   skills::error::ErrorCode::UserHasNoMutexError, {skillId}),
                               "SkillManagerComponentPluginUser",
                               __FUNCTION__,
                               __LINE__);
            throw error.toManagerIce();
        }

        // check if parameter exists in skill
        const auto& p = skill->second.parameters.find(parameterId);
        if (p == skill->second.parameters.end())
        {
            ARMARX_WARNING << "Parameter with id " << parameterId << " not found in skill with id "
                           << skillId;
            return false;
        }

        // update values of parameter
        p->second.updateValuesFromIce(values, profilesMap);
        return true;
    }

    IceUtil::Optional<skills::manager::dto::FluxioIdentificatorList>
    SkillManagerComponentPluginUser::deleteSkill(const std::string& skillId,
                                                 const std::string& userId,
                                                 const bool dryRun,
                                                 const Ice::Current& current)
    {
        skills::manager::dto::FluxioIdentificatorList ret;
        skills::Result<std::experimental::observer_ptr<const skills::FluxioSkill>,
                       skills::error::FluxioException>
            res = this->plugin->getSkill(skillId);
        if (!res.isSuccess())
        {
            ARMARX_WARNING << "Error getting skill with id " << skillId;
            return {};
        }
        std::experimental::observer_ptr<const armarx::skills::FluxioSkill> skillptr =
            res.getResult();
        std::optional<skills::manager::arondto::FluxioSkill> opt = skillptr->toAronXml();
        if (!opt.has_value())
        {
            ARMARX_WARNING << "Skill with id " << skillId << " could not be converted";
            return {};
        }
        skills::manager::arondto::FluxioSkill skill = opt.value();

        auto l = this->plugin->deleteSkill(skillId, userId, dryRun);

        if (!l.has_value())
        {
            return {};
        }

        for (const auto& s : l.value())
        {
            if (s == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr!";
                continue;
            }
            ret.push_back(s->toFluxioIdentificatorIce());
        }

        if (!dryRun)
        {
            skill.deleted = true;
            saveSkill(skill);
        }

        return ret;
    }

    bool
    SkillManagerComponentPluginUser::getSkillMutex(const std::string& skillId,
                                                   const std::string& userId,
                                                   const Ice::Current& current)
    {
        return this->plugin->getSkillMutex(skillId, userId).getResult(); //
    }

    void
    SkillManagerComponentPluginUser::deleteSkillMutex(const std::string& skillId,
                                                      const std::string& userId,
                                                      const Ice::Current& current)
    {
        this->plugin->deleteSkillMutex(skillId, userId);
    }

    void
    SkillManagerComponentPluginUser::removeSkillParameter(const std::string& userId,
                                                          const std::string& skillId,
                                                          const std::string& parameterId,
                                                          const Ice::Current& current)
    {
        this->plugin->removeSkillParameter(userId, skillId, parameterId);
    }

    skills::manager::dto::FluxioProfileList
    SkillManagerComponentPluginUser::getProfileList(const Ice::Current& current)
    {
        skills::manager::dto::FluxioProfileList ret;

        auto l = this->plugin->getProfileList();

        for (const auto& p : l.getResult())
        {
            if (p == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr!";
                continue;
            }
            ret.push_back(p->toManagerIce());
        }

        return ret;
    }

    IceUtil::Optional<skills::manager::dto::FluxioProfile>
    SkillManagerComponentPluginUser::getProfile(const std::string& id, const Ice::Current& current)
    {
        auto profile = this->plugin->getProfile(id);

        if (!profile.isSuccess())
        {
            auto e = profile.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }
        return profile.getResult().toManagerIce();
    }

    skills::manager::dto::FluxioProfile
    SkillManagerComponentPluginUser::createProfile(
        const skills::manager::dto::FluxioProfile& profile,
        const Ice::Current& current)
    {
        std::unique_lock l(this->plugin->fluxioDC.profilesMutex);
        auto& profilesMap = this->plugin->fluxioDC.profiles;
        l.unlock();

        armarx::skills::FluxioProfile ret =
            this->plugin->createProfile(skills::FluxioProfile::FromIce(profile, profilesMap))
                .getResult();

        addProfile(ret.toManagerAron());

        return ret.toManagerIce();
        ;
    }

    void
    SkillManagerComponentPluginUser::updateProfile(
        const skills::manager::dto::FluxioProfile& profile,
        const Ice::Current& current)
    {
        std::unique_lock l(this->plugin->fluxioDC.profilesMutex);
        auto& profilesMap = this->plugin->fluxioDC.profiles;
        l.unlock();

        this->plugin->updateProfile(skills::FluxioProfile::FromIce(profile, profilesMap));
    }

    skills::manager::dto::FluxioProviderList
    SkillManagerComponentPluginUser::getProviderList(const Ice::Current& current)
    {
        skills::manager::dto::FluxioProviderList ret;

        auto l = this->plugin->getProviderList();
        if (!l.isSuccess())
        {
            auto e = l.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }

        for (const auto& p : l.getResult())
        {
            if (p == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr!";
                continue;
            }
            ret.push_back(p->toManagerIce());
        }

        return ret;
    }

    IceUtil::Optional<skills::manager::dto::FluxioProvider>
    SkillManagerComponentPluginUser::getProvider(const std::string& id, const Ice::Current& current)
    {
        auto provider = this->plugin->getProvider(id);

        if (provider.isSuccess())
        {
            auto e = provider.getError();
            e.addToContext(std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw e.toManagerIce();
        }
        return provider.getResult().toManagerIce();
    }

    IceUtil::Optional<skills::manager::dto::FluxioSkillList>
    SkillManagerComponentPluginUser::getSkillsOfProvider(const std::string& id,
                                                         const Ice::Current& current)
    {
        skills::manager::dto::FluxioSkillList ret;

        auto l = this->plugin->getSkillsOfProvider(id);

        if (!l.isSuccess())
        {
            auto error = l.getError();
            error.addToContext(
                std::nullopt, "SkillManagerComponentPluginUser", __FUNCTION__, __LINE__);
            throw error.toManagerIce();
        }

        for (const auto& s : l.getResult())
        {
            if (s == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr!";
                continue;
            }

            const auto& skill = s->toManagerIce();

            if (!skill.has_value())
            {
                ARMARX_WARNING
                    << "SkillManagerComponentPluginUser::getSkillsOfProvider: Skill with id "
                    << s->id << " could not be converted";
                continue;
            }

            ret.push_back(skill.value());
        }

        return ret;
    }

    IceUtil::Optional<skills::manager::dto::FluxioSkill>
    SkillManagerComponentPluginUser::addSkillToProvider(
        const std::string& userId,
        const std::string& providerId,
        const skills::manager::dto::FluxioSkill& skill,
        const Ice::Current& current)
    {
        std::unique_lock skillsLock(this->plugin->fluxioDC.skillsMutex, std::defer_lock);
        std::unique_lock profilesLock(this->plugin->fluxioDC.profilesMutex, std::defer_lock);
        std::unique_lock providersLock(this->plugin->fluxioDC.providersMutex, std::defer_lock);
        std::unique_lock typesLock(this->plugin->fluxioDC.typesMutex, std::defer_lock);
        std::lock(skillsLock, profilesLock, providersLock, typesLock);

        auto& skillsMap = this->plugin->fluxioDC.skills;
        auto& providersMap = this->plugin->fluxioDC.providers;
        auto& profilesMap = this->plugin->fluxioDC.profiles;
        auto& typesMap = this->plugin->fluxioDC.types;
        auto skillBO =
            skills::FluxioSkill::FromIce(skill, providersMap, profilesMap, skillsMap, typesMap);

        if (skillBO == nullptr)
        {
            ARMARX_WARNING << "Skill with id " << skill.id << " could not be converted";

            throw skills::error::FluxioException::create(
                skills::error::createErrorMessage(skills::error::ErrorCode::ConverterError,
                                                  {skill.id}),
                skills::error::FluxioExceptionType::BAD_REQUEST,
                "SkillManagerComponentPluginUser",
                __FUNCTION__,
                __LINE__)
                .toManagerIce();
        }

        skillsLock.unlock();
        profilesLock.unlock();
        providersLock.unlock();
        typesLock.unlock();

        auto& skillReleased = *skillBO.release();
        const auto res =
            this->plugin->addSkillToProvider(userId, providerId, std::move(skillReleased));
        if (!res.isSuccess())
        {
            ARMARX_WARNING << "Skill with id " << skill.id
                           << " could not be added to provider with id " << providerId;

            auto error = res.getError();
            error.addToContext(
                (skills::error::createErrorMessage(skills::error::ErrorCode::AddSkillError,
                                                   {skill.id, providerId})),
                "SkillManagerComponentPluginUser",
                __FUNCTION__,
                __LINE__);
            throw error.toManagerIce();
        }

        const auto& s = res.getResult();

        if (s == nullptr)
        {
            ARMARX_WARNING << "Skill with id " << skill.id
                           << " could not be added to provider with id " << providerId;
            return {};
        }

        const auto& ret = s->toManagerIce();
        if (!ret.has_value())
        {
            ARMARX_WARNING << "Skill with id " << skill.id << " could not be converted";
            return {};
        }

        const std::optional<skills::manager::arondto::FluxioSkill> aronSkill = s->toAronXml();

        if (!aronSkill.has_value())
        {
            ARMARX_WARNING << "Skill with id " << skill.id << " could not be converted to Aron";
            return {};
        }

        saveSkill(aronSkill.value());

        return ret.value();
    }

    void
    SkillManagerComponentPluginUser::saveSkill(const skills::manager::arondto::FluxioSkill& skill)
    {
        // Implemented in derived class
    }

    std::optional<std::vector<skills::manager::arondto::FluxioSkill>>
    SkillManagerComponentPluginUser::loadCompositeSkills()
    {
        // Implemented in derived class
        return {};
    }

    std::optional<std::vector<skills::manager::arondto::FluxioSkill>>
    SkillManagerComponentPluginUser::loadCompositeSkillsOfProvider(const std::string& providerId)
    {
        // Implemented in derived class
        return {};
    }

    void
    SkillManagerComponentPluginUser::addProfile(
        const skills::manager::arondto::FluxioProfile& profile)
    {
        // Implemented in derived class
    }

    std::optional<std::vector<skills::manager::arondto::FluxioProfile>>
    SkillManagerComponentPluginUser::loadProfiles()
    {
        // Implemented in derived class
        return {};
    }
} // namespace armarx