util.h

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#pragma once

#include <vector>
#include <optional>

#include <ArmarXCore/core/logging/Logging.h>

#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>


namespace armarx::armem
{

    /**
     * @brief Tries to cast a armem::EntityInstance to AronClass
     *
     * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
     * @param item
     * @return std::optional<AronClass>
     */
    template <typename AronClass>
    std::optional<AronClass> tryCast(const wm::EntityInstance& item)
    {
        static_assert(std::is_base_of<armarx::aron::cpp::AronGeneratedClass, AronClass>::value);


        if (!item.data())
        {
            return std::nullopt;
        }

#if 0
        // item.data() is by definition a DictNavigator
        if (item.data()->getDescriptor() != aron::data::Descriptor::DICT)
        {
            return std::nullopt;
        }
#endif

        try
        {
            return AronClass::FromAron(item.data());
        }
        catch (const armarx::aron::error::AronException&)
        {
            return std::nullopt;
        }
    }

    /**
     * @brief Returns all entities that can be cast to AronClass
     *
     * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
     * @param entities collection of entities
     * @return std::vector<AronClass>
     */
    template <typename AronClass>
    std::vector<AronClass>
    allOfType(const std::map<std::string, wm::Entity>& entities)
    {
        static_assert(std::is_base_of<armarx::aron::cpp::AronGeneratedClass, AronClass>::value);

        std::vector<AronClass> outV;

        // loop over all entities and their snapshots
        for (const auto &[s, entity] : entities)
        {
            entity.forEachInstance([&outV](const wm::EntityInstance& entityInstance)
            {
                const auto o = tryCast<AronClass>(entityInstance);

                if (o)
                {
                    outV.push_back(*o);
                }
            });
        }

        return outV;
    }

    /**
     * @brief filter + transform for entities.
     *
     * Can be used instead of
     *
     *      std::vector<Bar> ret;
     *
     *      const auto allOf = allOfType<Foo>(entities);
     *      std::transform(allOf.begin(), allOf.end(), std::back_inserter(ret), pred)
     *
     * This function has the benefit that the transform function will be applied directly.
     * No intermediate vector has to be created (e.g. "allOf" in the example above).
     *
     * @tparam AronClass class name. Needs to be derived from armarx::aron::cppcodegenerator::AronCppClass
     * @param entities collection of entities
     * @param pred binary predicate function, applied to all entity instances
     * @return vector of "pred"-transformed elements that can be cast to AronClass
     */
    template <typename AronClass>
    auto transformAllOfType(const std::map<std::string, wm::Entity>& entities,
                            auto pred) -> std::vector<decltype(pred(AronClass()))>
    {
        static_assert(std::is_base_of<armarx::aron::cpp::AronGeneratedClass, AronClass>::value);

        std::vector<decltype(pred(AronClass()))> outV;

        if (entities.empty())
        {
            ARMARX_WARNING << "No entities!";
        }

        // loop over all entities and their snapshots
        for (const auto &[s, entity] : entities)
        {
            entity.forEachInstance([&outV, &pred](const wm::EntityInstance& entityInstance)
            {
                const auto o = tryCast<AronClass>(entityInstance);

                if (o)
                {
                    outV.push_back(pred(*o));
                }
            });
        }

        return outV;
    }

    /**
     * @brief resolve a single MemoryID with the given MemoryNameSystem.
     *
     * Returns a Memory containing only the desired segment / entity if it was
     * successfully queried. If the query was unsuccessful (the MNS could not
     * resolve the memory server, the MemoryID does not exist, etc.),
     * nothing is returned.
     *
     * @param mns the MemoryNameSystem to use for the query
     * @param memoryID the MemoryID to query for
     * @return memory containing the object referenced by the MemoryID if available
     */
    std::optional<armarx::armem::wm::Memory>
    resolveID(armarx::armem::client::MemoryNameSystem& mns,
              const armarx::armem::MemoryID& memoryID);


    /**
     * @brief get the data and type of the given MemoryID in the given Memory.
     *
     * Returns the data and the type of the latest instance corresponding to
     * the given MemoryID (whatever `memory.findLatestInstance(memoryID)` returns).
     * If no such instance exists in the memory or no type for the instance is available,
     * nothing is returned.
     *
     * @param memory the Memory to extract the data and type from
     * @param memoryID the MemoryID of the instance to extract
     * @return pair of data and type for the instance if available
     */
    std::optional<std::pair<armarx::aron::data::DictPtr, armarx::aron::type::ObjectPtr>>
    extractDataAndType(const armarx::armem::wm::Memory& memory,
                       const armarx::armem::MemoryID& memoryID);

} // namespace armarx::armem