EntitySnapshotBase.h

Go to the documentation of this file.

#pragma once
 
#include <vector>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
 
#include "EntityInstanceBase.h"
#include "detail/MemoryContainerBase.h"
#include "detail/iteration_mixins.h"
#include "detail/lookup_mixins.h"
 
namespace armarx::armem::base::detail
{
    void throwIfNotEqual(const Time& ownTime, const Time& updateTime);
}
 
namespace armarx::armem::base
{
    /**
     * @brief Data of an entity at one point in time.
     */
    template <class _EntityInstanceT, class _Derived>
    class EntitySnapshotBase :
        public detail::MemoryContainerBase<std::vector<_EntityInstanceT>, _Derived>
    {
        using Base = detail::MemoryContainerBase<std::vector<_EntityInstanceT>, _Derived>;
 
    public:
        using typename Base::ContainerT;
        using typename Base::DerivedT;
 
        using EntityInstanceT = _EntityInstanceT;
 
 
    public:
        EntitySnapshotBase()
        {
        }
 
        explicit EntitySnapshotBase(Time time, const MemoryID& parentID = {}) :
            EntitySnapshotBase(parentID.withTimestamp(time))
        {
        }
 
        explicit EntitySnapshotBase(const MemoryID& id) : Base(id)
        {
        }
 
        EntitySnapshotBase(const EntitySnapshotBase& other) = default;
        EntitySnapshotBase(EntitySnapshotBase&& other) = default;
        EntitySnapshotBase& operator=(const EntitySnapshotBase& other) = default;
        EntitySnapshotBase& operator=(EntitySnapshotBase&& other) = default;
 
        // READING
 
        // Get key
        inline Time&
        time()
        {
            return this->id().timestamp;
        }
 
        inline const Time&
        time() const
        {
            return this->id().timestamp;
        }
 
        /// Indicate whether this snapshot has any instances.
        bool
        hasInstances() const
        {
            return not this->empty();
        }
 
        // Has child by key
        bool
        hasInstance(int index) const
        {
            return this->findInstance(index) != nullptr;
        }
 
        // Has child by ID
        bool
        hasInstance(const MemoryID& instanceID) const
        {
            return this->findInstance(instanceID) != nullptr;
        }
 
        // Find child by key
        EntityInstanceT*
        findInstance(int index)
        {
            return const_cast<EntityInstanceT*>(
                const_cast<const EntitySnapshotBase*>(this)->findInstance(index));
        }
 
        const EntityInstanceT*
        findInstance(int index) const
        {
            const size_t si = static_cast<size_t>(index);
            return (index >= 0 && si < this->_container.size()) ? &this->_container[si] : nullptr;
        }
 
        // Get child by key
        /**
         * @brief Get the given instance.
         * @param index The instance's index.
         * @return The instance.
         * @throw `armem::error::MissingEntry` If the given index is invalid.
         */
        EntityInstanceT&
        getInstance(int index)
        {
            return const_cast<EntityInstanceT&>(
                const_cast<const EntitySnapshotBase*>(this)->getInstance(index));
        }
 
        const EntityInstanceT&
        getInstance(int index) const
        {
            if (const EntityInstanceT* instance = findInstance(index))
            {
                return *instance;
            }
            else
            {
                throw armem::error::MissingEntry::create<EntityInstanceT>(std::to_string(index),
                                                                          *this);
            }
        }
 
        // Find child by MemoryID
        EntityInstanceT*
        findInstance(const MemoryID& instanceID)
        {
            detail::checkHasInstanceIndex(instanceID);
            return this->findInstance(instanceID.instanceIndex);
        }
 
        const EntityInstanceT*
        findInstance(const MemoryID& instanceID) const
        {
            detail::checkHasInstanceIndex(instanceID);
            return this->findInstance(instanceID.instanceIndex);
        }
 
        // Get child by MemoryID
        /**
         * @brief Get the given instance.
         * @param index The instance's index.
         * @return The instance.
         * @throw `armem::error::MissingEntry` If the given index is invalid.
         * @throw `armem::error::InvalidMemoryID` If memory ID does not have an instance index.
         */
        EntityInstanceT&
        getInstance(const MemoryID& instanceID)
        {
            detail::checkHasInstanceIndex(instanceID);
            return this->getInstance(instanceID.instanceIndex);
        }
 
        const EntityInstanceT&
        getInstance(const MemoryID& instanceID) const
        {
            detail::checkHasInstanceIndex(instanceID);
            return this->getInstance(instanceID.instanceIndex);
        }
 
        // ITERATION
 
        /**
         * @param func Function like void process(EntityInstanceT& instance)>
         */
        template <class InstanceFunctionT>
        bool
        forEachInstance(InstanceFunctionT&& func)
        {
            return this->forEachChild(func);
        }
 
        /**
         * @param func Function like void process (const EntityInstanceT& instance)
         */
        template <class InstanceFunctionT>
        bool
        forEachInstance(InstanceFunctionT&& func) const
        {
            return this->forEachChild(func);
        }
 
        /**
         * @param func Function like void process(EntityInstanceT& instance)>
         */
        template <class InstanceFunctionT>
        bool
        forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func)
        {
            if (id.hasInstanceIndex())
            {
                EntityInstanceT* child = findInstance(id.instanceIndex);
                return child ? base::detail::call(func, *child) : true;
            }
            else
            {
                return this->forEachInstance(func);
            }
        }
 
        /**
         * @param func Function like void process (const EntityInstanceT& instance)
         */
        template <class InstanceFunctionT>
        bool
        forEachInstanceIn(const MemoryID& id, InstanceFunctionT&& func) const
        {
            if (id.hasInstanceIndex())
            {
                const EntityInstanceT* child = findInstance(id.instanceIndex);
                return child ? base::detail::call(func, *child) : true;
            }
            else
            {
                return this->forEachInstance(func);
            }
        }
 
        // Get child keys
        std::vector<int>
        getInstanceIndices() const
        {
            std::vector<int> indices;
            indices.reserve(this->size());
            for (size_t i = 0; i < this->size(); ++i)
            {
                indices.push_back(static_cast<int>(i));
            }
            return indices;
        }
 
        // MODIFICATION
 
        void
        update(const EntityUpdate& update)
        {
            detail::throwIfNotEqual(time(), update.referencedTime);
 
            this->_container.clear();
            for (int index = 0; index < int(update.instancesData.size()); ++index)
            {
                EntityInstanceT& data = this->_container.emplace_back(index, this->id());
                data.update(update);
            }
        }
 
        /**
         * @brief Add a single instance with data.
         * @param instance The instance.
         * @return The stored instance.
         * @throw `armem::error::InvalidArgument` If the given index is invalid. Must be equal to container.size() or -1 (meaning push_back)
         */
        EntityInstanceT&
        addInstance(const EntityInstanceT& instance)
        {
            return addInstance(EntityInstanceT(instance));
        }
 
        EntityInstanceT&
        addInstance(EntityInstanceT&& instance)
        {
            if (instance.index() > 0 && static_cast<size_t>(instance.index()) > this->_container.size())
            {
                throw error::InvalidArgument(
                    std::to_string(instance.index()),
                    "EntitySnapshot::addInstance",
                    "Cannot add an EntityInstance because its index is too big.");
            }
 
            int index = instance.index();
            EntityInstanceT& added = this->_container.emplace_back(std::move(instance));
 
            added.id() = this->id().withInstanceIndex(index);
            return added;
        }
 
        EntityInstanceT&
        addInstance()
        {
            //ARMARX_INFO << "Trying to add an instance without instance yet generated";
            int index = static_cast<int>(this->size()); //this is the problem, because instances have names 0 and 11 (I do not know why)
            EntityInstanceT& added = this->_container.emplace_back(EntityInstanceT());
            added.id() = this->id().withInstanceIndex(index);
            return added;
        }
 
        // MISC
 
        bool
        equalsDeep(const DerivedT& other) const
        {
            //std::cout << "EntitySnapshot::equalsDeep" << std::endl;
            if (this->size() != other.size())
            {
                return false;
            }
            int i = 0;
            for (const auto& instance : this->_container)
            {
                if (not instance.equalsDeep(other.getInstance(i)))
                {
                    return false;
                }
                i++;
            }
            return true;
        }
 
        std::string
        getKeyString() const
        {
            return toDateTimeMilliSeconds(this->time());
        }
 
        static std::string
        getLevelName()
        {
            return "entity snapshot";
        }
    };
 
} // namespace armarx::armem::base