Manager.hpp

 
// This work derives from Vittorio Romeo's code used for cppcon 2015 licensed
// under the Academic Free License.
// His code is available here: https://github.com/SuperV1234/cppcon2015
 
#ifndef EC_MANAGER_HPP
#define EC_MANAGER_HPP
 
#define EC_INIT_ENTITIES_SIZE 256
#define EC_GROW_SIZE_AMOUNT 256
 
#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cstddef>
#include <deque>
#include <functional>
#include <map>
#include <mutex>
#include <set>
#include <thread>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
 
#ifndef NDEBUG
#include <iostream>
#endif
 
#include "Bitset.hpp"
#include "Meta/Combine.hpp"
#include "Meta/ForEachDoubleTuple.hpp"
#include "Meta/ForEachWithIndex.hpp"
#include "Meta/IndexOf.hpp"
#include "Meta/Matching.hpp"
#include "ThreadPool.hpp"
 
namespace EC {
template <typename ComponentsList, typename TagsList,
          unsigned int ThreadCount = 4>
struct Manager {
   public:
    using Components = ComponentsList;
    using Tags = TagsList;
    using Combined = EC::Meta::Combine<ComponentsList, TagsList>;
    using BitsetType = EC::Bitset<ComponentsList, TagsList>;
 
   private:
    using ComponentsTuple = EC::Meta::Morph<ComponentsList, std::tuple<> >;
    static_assert(std::is_default_constructible<ComponentsTuple>::value,
                  "All components must be default constructible");
 
    template <typename... Types>
    struct Storage {
        using type = std::tuple<std::deque<Types>..., std::deque<char> >;
    };
    using ComponentsStorage =
        typename EC::Meta::Morph<ComponentsList, Storage<> >::type;
 
    // Entity: isAlive, ComponentsTags Info
    using EntitiesTupleType = std::tuple<bool, BitsetType>;
    using EntitiesType = std::deque<EntitiesTupleType>;
 
    EntitiesType entities;
    ComponentsStorage componentsStorage;
    std::size_t currentCapacity = 0;
    std::size_t currentSize = 0;
    std::unordered_set<std::size_t> deletedSet;
 
    std::unique_ptr<ThreadPool<ThreadCount> > threadPool;
 
    std::atomic_uint deferringDeletions;
    std::vector<std::size_t> deferredDeletions;
    std::mutex deferredDeletionsMutex;
 
    std::vector<std::size_t> idStack;
    std::size_t idStackCounter;
    std::mutex idStackMutex;
 
   public:
    // section for "temporary" structures {{{
    struct TPFnDataStructZero {
        std::array<std::size_t, 2> range;
        Manager* manager;
        EntitiesType* entities;
        const BitsetType* signature;
        void* userData;
        std::unordered_set<std::size_t> dead;
    };
    template <typename Function>
    struct TPFnDataStructOne {
        std::array<std::size_t, 2> range;
        Manager* manager;
        EntitiesType* entities;
        BitsetType* signature;
        void* userData;
        Function* fn;
        std::unordered_set<std::size_t> dead;
    };
    struct TPFnDataStructTwo {
        std::array<std::size_t, 2> range;
        Manager* manager;
        EntitiesType* entities;
        void* userData;
        const std::vector<std::size_t>* matching;
        std::unordered_set<std::size_t> dead;
    };
    struct TPFnDataStructThree {
        std::array<std::size_t, 2> range;
        Manager* manager;
        std::vector<std::vector<std::size_t> >* matchingV;
        const std::vector<BitsetType*>* bitsets;
        EntitiesType* entities;
        std::mutex* mutex;
        std::unordered_set<std::size_t> dead;
    };
    struct TPFnDataStructFour {
        std::array<std::size_t, 2> range;
        Manager* manager;
        std::vector<std::vector<std::size_t> >* multiMatchingEntities;
        BitsetType* signatures;
        std::mutex* mutex;
        std::unordered_set<std::size_t> dead;
    };
    struct TPFnDataStructFive {
        std::array<std::size_t, 2> range;
        std::size_t index;
        Manager* manager;
        void* userData;
        std::vector<std::vector<std::size_t> >* multiMatchingEntities;
        std::unordered_set<std::size_t> dead;
    };
    struct TPFnDataStructSix {
        std::array<std::size_t, 2> range;
        Manager* manager;
        std::vector<std::vector<std::size_t> >* multiMatchingEntities;
        BitsetType* bitsets;
        std::mutex* mutex;
        std::unordered_set<std::size_t> dead;
    };
    template <typename Iterable>
    struct TPFnDataStructSeven {
        std::array<std::size_t, 2> range;
        Manager* manager;
        EntitiesType* entities;
        Iterable* iterable;
        void* userData;
        std::unordered_set<std::size_t> dead;
    };
    // end section for "temporary" structures }}}
 
    Manager() : threadPool{}, idStackCounter(0) {
        resize(EC_INIT_ENTITIES_SIZE);
        if (ThreadCount >= 2) {
            threadPool = std::make_unique<ThreadPool<ThreadCount> >();
        }
 
        deferringDeletions.store(0);
    }
 
    ~Manager() {
        if (threadPool) {
            while (!threadPool->isNotRunning()) {
                std::this_thread::sleep_for(std::chrono::microseconds(30));
            }
        }
    }
 
   private:
    void resize(std::size_t newCapacity) {
        if (currentCapacity >= newCapacity) {
            return;
        }
 
        EC::Meta::forEach<ComponentsList>([this, newCapacity](auto t) {
            std::get<std::deque<decltype(t)> >(this->componentsStorage)
                .resize(newCapacity);
        });
 
        entities.resize(newCapacity);
        for (std::size_t i = currentCapacity; i < newCapacity; ++i) {
            entities[i] = std::make_tuple(false, BitsetType{});
        }
 
        currentCapacity = newCapacity;
    }
 
   public:
    std::size_t addEntity() {
        if (deletedSet.empty()) {
            if (currentSize == currentCapacity) {
                resize(currentCapacity + EC_GROW_SIZE_AMOUNT);
            }
 
            std::get<bool>(entities[currentSize]) = true;
 
            return currentSize++;
        } else {
            std::size_t id;
            {
                auto iter = deletedSet.begin();
                id = *iter;
                deletedSet.erase(iter);
            }
            std::get<bool>(entities[id]) = true;
            return id;
        }
    }
 
   private:
    void deleteEntityImpl(std::size_t id) {
        if (hasEntity(id)) {
            std::get<bool>(entities.at(id)) = false;
            std::get<BitsetType>(entities.at(id)).reset();
            deletedSet.insert(id);
        }
    }
 
   public:
    void deleteEntity(std::size_t index) {
        if (deferringDeletions.load() != 0) {
            std::lock_guard<std::mutex> lock(deferredDeletionsMutex);
            deferredDeletions.push_back(index);
        } else {
            deleteEntityImpl(index);
        }
    }
 
   private:
    void handleDeferredDeletions() {
        if (deferringDeletions.fetch_sub(1) == 1) {
            std::lock_guard<std::mutex> lock(deferredDeletionsMutex);
            for (std::size_t id : deferredDeletions) {
                deleteEntityImpl(id);
            }
            deferredDeletions.clear();
        }
    }
 
   public:
    bool hasEntity(const std::size_t& index) const {
        return index < currentSize;
    }
 
    bool isAlive(const std::size_t& index) const {
        return hasEntity(index) && std::get<bool>(entities.at(index));
    }
 
    std::size_t getCurrentSize() const {
        return currentSize - deletedSet.size();
    }
 
    /*
        \brief Returns the current capacity or number of entities the system
            can hold.
 
        Note that when capacity is exceeded, the capacity is increased by
        EC_GROW_SIZE_AMOUNT.
    */
    std::size_t getCurrentCapacity() const { return currentCapacity; }
 
    const EntitiesTupleType& getEntityInfo(const std::size_t& index) const {
        return entities.at(index);
    }
 
    template <typename Component>
    Component* getEntityData(const std::size_t& index) {
        constexpr auto componentIndex =
            EC::Meta::IndexOf<Component, Components>::value;
        if (componentIndex < Components::size) {
            // Cast required due to compiler thinking that an invalid
            // Component is needed even though the enclosing if statement
            // prevents this from ever happening.
            return (Component*)&std::get<componentIndex>(componentsStorage)
                .at(index);
        } else {
            return nullptr;
        }
    }
 
    template <typename Component>
    Component* getEntityComponent(const std::size_t& index) {
        return getEntityData<Component>(index);
    }
 
    template <typename Component>
    const Component* getEntityData(const std::size_t& index) const {
        constexpr auto componentIndex =
            EC::Meta::IndexOf<Component, Components>::value;
        if (componentIndex < Components::size) {
            // Cast required due to compiler thinking that an invalid
            // Component is needed even though the enclosing if statement
            // prevents this from ever happening.
            return (Component*)&std::get<componentIndex>(componentsStorage)
                .at(index);
        } else {
            return nullptr;
        }
    }
 
    template <typename Component>
    const Component* getEntityComponent(const std::size_t& index) const {
        return getEntityData<Component>(index);
    }
 
    template <typename Component>
    bool hasComponent(const std::size_t& index) const {
        return std::get<BitsetType>(entities.at(index))
            .template getComponentBit<Component>();
    }
 
    template <typename Tag>
    bool hasTag(const std::size_t& index) const {
        return std::get<BitsetType>(entities.at(index))
            .template getTagBit<Tag>();
    }
 
    template <typename Component, typename... Args>
    void addComponent(const std::size_t& entityID, Args&&... args) {
        if (!EC::Meta::Contains<Component, Components>::value ||
            !isAlive(entityID)) {
            return;
        }
 
        Component component(std::forward<Args>(args)...);
 
        std::get<BitsetType>(entities[entityID])
            .template getComponentBit<Component>() = true;
 
        constexpr auto index = EC::Meta::IndexOf<Component, Components>::value;
 
        // Cast required due to compiler thinking that deque<char> at
        // index = Components::size is being used, even if the previous
        // if statement will prevent this from ever happening.
        (*((std::deque<Component>*)(&std::get<index>(
            componentsStorage))))[entityID] = std::move(component);
    }
 
    template <typename Component>
    void removeComponent(const std::size_t& entityID) {
        if (!EC::Meta::Contains<Component, Components>::value ||
            !isAlive(entityID)) {
            return;
        }
 
        std::get<BitsetType>(entities[entityID])
            .template getComponentBit<Component>() = false;
    }
 
    template <typename Tag>
    void addTag(const std::size_t& entityID) {
        if (!EC::Meta::Contains<Tag, Tags>::value || !isAlive(entityID)) {
            return;
        }
 
        std::get<BitsetType>(entities[entityID]).template getTagBit<Tag>() =
            true;
    }
 
    template <typename Tag>
    void removeTag(const std::size_t& entityID) {
        if (!EC::Meta::Contains<Tag, Tags>::value || !isAlive(entityID)) {
            return;
        }
 
        std::get<BitsetType>(entities[entityID]).template getTagBit<Tag>() =
            false;
    }
 
    void reset() {
        clearForMatchingFunctions();
 
        currentSize = 0;
        currentCapacity = 0;
        deletedSet.clear();
        resize(EC_INIT_ENTITIES_SIZE);
 
        std::lock_guard<std::mutex> lock(deferredDeletionsMutex);
        deferringDeletions.store(0);
        deferredDeletions.clear();
    }
 
   private:
    template <typename... Types>
    struct ForMatchingSignatureHelper {
        template <typename CType, typename Function>
        static void call(const std::size_t& entityID, CType& ctype,
                         Function&& function, void* userData = nullptr) {
            function(entityID, userData,
                     ctype.template getEntityData<Types>(entityID)...);
        }
 
        template <typename CType, typename Function>
        static void callPtr(const std::size_t& entityID, CType& ctype,
                            Function* function, void* userData = nullptr) {
            (*function)(entityID, userData,
                        ctype.template getEntityData<Types>(entityID)...);
        }
 
        template <typename CType, typename Function>
        void callInstance(const std::size_t& entityID, CType& ctype,
                          Function&& function, void* userData = nullptr) const {
            ForMatchingSignatureHelper<Types...>::call(
                entityID, ctype, std::forward<Function>(function), userData);
        }
 
        template <typename CType, typename Function>
        void callInstancePtr(const std::size_t& entityID, CType& ctype,
                             Function* function,
                             void* userData = nullptr) const {
            ForMatchingSignatureHelper<Types...>::callPtr(entityID, ctype,
                                                          function, userData);
        }
    };
 
   public:
    template <typename Signature, typename Function>
    void forMatchingSignature(Function&& function, void* userData = nullptr,
                              const bool useThreadPool = false) {
        std::size_t current_id;
        {
            // push to idStack "call stack"
            std::lock_guard<std::mutex> lock(idStackMutex);
            current_id = idStackCounter++;
            idStack.push_back(current_id);
        }
        deferringDeletions.fetch_add(1);
        using SignatureComponents =
            typename EC::Meta::Matching<Signature, ComponentsList>::type;
        using Helper =
            EC::Meta::Morph<SignatureComponents, ForMatchingSignatureHelper<> >;
 
        BitsetType signatureBitset =
            BitsetType::template generateBitset<Signature>();
        if (!useThreadPool || !threadPool) {
            for (std::size_t i = 0; i < currentSize; ++i) {
                if (!std::get<bool>(entities[i])) {
                    continue;
                }
 
                if ((signatureBitset & std::get<BitsetType>(entities[i])) ==
                    signatureBitset) {
                    Helper::call(i, *this, std::forward<Function>(function),
                                 userData);
                }
            }
        } else {
            std::array<TPFnDataStructZero, ThreadCount * 2> fnDataAr;
 
            std::size_t s = currentSize / (ThreadCount * 2);
            for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                std::size_t begin = s * i;
                std::size_t end;
                if (i == ThreadCount * 2 - 1) {
                    end = currentSize;
                } else {
                    end = s * (i + 1);
                }
                if (begin == end) {
                    continue;
                }
                fnDataAr[i].range = {begin, end};
                fnDataAr[i].manager = this;
                fnDataAr[i].entities = &entities;
                fnDataAr[i].signature = &signatureBitset;
                fnDataAr[i].userData = userData;
                for (std::size_t j = begin; j < end; ++j) {
                    if (!isAlive(j)) {
                        fnDataAr[i].dead.insert(j);
                    }
                }
 
                threadPool->queueFn(
                    [&function](void* ud) {
                        auto* data = static_cast<TPFnDataStructZero*>(ud);
                        for (std::size_t i = data->range[0]; i < data->range[1];
                             ++i) {
                            if (data->dead.find(i) != data->dead.end()) {
                                continue;
                            }
 
                            if (((*data->signature) &
                                 std::get<BitsetType>(data->entities->at(i))) ==
                                *data->signature) {
                                Helper::call(i, *data->manager,
                                             std::forward<Function>(function),
                                             data->userData);
                            }
                        }
                    },
                    &fnDataAr[i]);
            }
            threadPool->easyStartAndWait();
        }
 
        // pop from idStack "call stack"
        do {
            {
                std::lock_guard<std::mutex> lock(idStackMutex);
                if (idStack.back() == current_id) {
                    idStack.pop_back();
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::microseconds(15));
        } while (true);
 
        handleDeferredDeletions();
    }
 
    template <typename Signature, typename Function>
    void forMatchingSignaturePtr(Function* function, void* userData = nullptr,
                                 const bool useThreadPool = false) {
        std::size_t current_id;
        {
            // push to idStack "call stack"
            std::lock_guard<std::mutex> lock(idStackMutex);
            current_id = idStackCounter++;
            idStack.push_back(current_id);
        }
        deferringDeletions.fetch_add(1);
        using SignatureComponents =
            typename EC::Meta::Matching<Signature, ComponentsList>::type;
        using Helper =
            EC::Meta::Morph<SignatureComponents, ForMatchingSignatureHelper<> >;
 
        BitsetType signatureBitset =
            BitsetType::template generateBitset<Signature>();
        if (!useThreadPool || !threadPool) {
            for (std::size_t i = 0; i < currentSize; ++i) {
                if (!std::get<bool>(entities[i])) {
                    continue;
                }
 
                if ((signatureBitset & std::get<BitsetType>(entities[i])) ==
                    signatureBitset) {
                    Helper::callPtr(i, *this, function, userData);
                }
            }
        } else {
            std::array<TPFnDataStructOne<Function>, ThreadCount * 2> fnDataAr;
 
            std::size_t s = currentSize / (ThreadCount * 2);
            for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                std::size_t begin = s * i;
                std::size_t end;
                if (i == ThreadCount * 2 - 1) {
                    end = currentSize;
                } else {
                    end = s * (i + 1);
                }
                if (begin == end) {
                    continue;
                }
                fnDataAr[i].range = {begin, end};
                fnDataAr[i].manager = this;
                fnDataAr[i].entities = &entities;
                fnDataAr[i].signature = &signatureBitset;
                fnDataAr[i].userData = userData;
                fnDataAr[i].fn = function;
                for (std::size_t j = begin; j < end; ++j) {
                    if (!isAlive(j)) {
                        fnDataAr[i].dead.insert(j);
                    }
                }
                threadPool->queueFn(
                    [](void* ud) {
                        auto* data =
                            static_cast<TPFnDataStructOne<Function>*>(ud);
                        for (std::size_t i = data->range[0]; i < data->range[1];
                             ++i) {
                            if (data->dead.find(i) != data->dead.end()) {
                                continue;
                            }
 
                            if (((*data->signature) &
                                 std::get<BitsetType>(data->entities->at(i))) ==
                                *data->signature) {
                                Helper::callPtr(i, *data->manager, data->fn,
                                                data->userData);
                            }
                        }
                    },
                    &fnDataAr[i]);
            }
            threadPool->easyStartAndWait();
        }
 
        // pop from idStack "call stack"
        do {
            {
                std::lock_guard<std::mutex> lock(idStackMutex);
                if (idStack.back() == current_id) {
                    idStack.pop_back();
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::microseconds(15));
        } while (true);
 
        handleDeferredDeletions();
    }
 
   private:
    std::map<std::size_t,
             std::tuple<BitsetType, void*,
                        std::function<void(std::size_t,
                                           std::vector<std::size_t>, void*)> > >
        forMatchingFunctions;
    std::size_t functionIndex = 0;
 
   public:
    template <typename Signature, typename Function>
    std::size_t addForMatchingFunction(Function&& function,
                                       void* userData = nullptr) {
        deferringDeletions.fetch_add(1);
        while (forMatchingFunctions.find(functionIndex) !=
               forMatchingFunctions.end()) {
            ++functionIndex;
        }
 
        using SignatureComponents =
            typename EC::Meta::Matching<Signature, ComponentsList>::type;
        using Helper =
            EC::Meta::Morph<SignatureComponents, ForMatchingSignatureHelper<> >;
 
        Helper helper;
        BitsetType signatureBitset =
            BitsetType::template generateBitset<Signature>();
 
        forMatchingFunctions.emplace(std::make_pair(
            functionIndex,
            std::make_tuple(
                signatureBitset, userData,
                [function, helper, this](const bool useThreadPool,
                                         std::vector<std::size_t> matching,
                                         void* userData) {
                    if (!useThreadPool || !threadPool) {
                        for (auto eid : matching) {
                            if (isAlive(eid)) {
                                helper.callInstancePtr(eid, *this, &function,
                                                       userData);
                            }
                        }
                    } else {
                        std::array<TPFnDataStructTwo, ThreadCount * 2> fnDataAr;
 
                        std::size_t s = matching.size() / (ThreadCount * 2);
                        for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                            std::size_t begin = s * i;
                            std::size_t end;
                            if (i == ThreadCount * 2 - 1) {
                                end = matching.size();
                            } else {
                                end = s * (i + 1);
                            }
                            if (begin == end) {
                                continue;
                            }
                            fnDataAr[i].range = {begin, end};
                            fnDataAr[i].manager = this;
                            fnDataAr[i].entities = &entities;
                            fnDataAr[i].userData = userData;
                            fnDataAr[i].matching = &matching;
                            for (std::size_t j = begin; j < end; ++j) {
                                if (!isAlive(matching.at(j))) {
                                    fnDataAr[i].dead.insert(j);
                                }
                            }
                            threadPool->queueFn(
                                [&function, helper](void* ud) {
                                    auto* data =
                                        static_cast<TPFnDataStructTwo*>(ud);
                                    for (std::size_t i = data->range[0];
                                         i < data->range[1]; ++i) {
                                        if (data->dead.find(i) ==
                                            data->dead.end()) {
                                            helper.callInstancePtr(
                                                data->matching->at(i),
                                                *data->manager, &function,
                                                data->userData);
                                        }
                                    }
                                },
                                &fnDataAr[i]);
                        }
                        threadPool->easyStartAndWait();
                    }
                })));
 
        handleDeferredDeletions();
        return functionIndex++;
    }
 
   private:
    std::vector<std::vector<std::size_t> > getMatchingEntities(
        std::vector<BitsetType*> bitsets, const bool useThreadPool = false) {
        std::vector<std::vector<std::size_t> > matchingV(bitsets.size());
 
        if (!useThreadPool || !threadPool) {
            for (std::size_t i = 0; i < currentSize; ++i) {
                if (!isAlive(i)) {
                    continue;
                }
                for (std::size_t j = 0; j < bitsets.size(); ++j) {
                    if (((*bitsets[j]) & std::get<BitsetType>(entities[i])) ==
                        (*bitsets[j])) {
                        matchingV[j].push_back(i);
                    }
                }
            }
        } else {
            std::array<TPFnDataStructThree, ThreadCount * 2> fnDataAr;
 
            std::size_t s = currentSize / (ThreadCount * 2);
            std::mutex mutex;
            for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                std::size_t begin = s * i;
                std::size_t end;
                if (i == ThreadCount * 2 - 1) {
                    end = currentSize;
                } else {
                    end = s * (i + 1);
                }
                if (begin == end) {
                    continue;
                }
                fnDataAr[i].range = {begin, end};
                fnDataAr[i].manager = this;
                fnDataAr[i].matchingV = &matchingV;
                fnDataAr[i].bitsets = &bitsets;
                fnDataAr[i].entities = &entities;
                fnDataAr[i].mutex = &mutex;
                for (std::size_t j = begin; j < end; ++j) {
                    if (!isAlive(j)) {
                        fnDataAr[i].dead.insert(j);
                    }
                }
                threadPool->queueFn(
                    [](void* ud) {
                        auto* data = static_cast<TPFnDataStructThree*>(ud);
                        for (std::size_t i = data->range[0]; i < data->range[1];
                             ++i) {
                            if (data->dead.find(i) != data->dead.end()) {
                                continue;
                            }
                            for (std::size_t j = 0; j < data->bitsets->size();
                                 ++j) {
                                if ((*data->bitsets->at(j) &
                                     std::get<BitsetType>(data->entities->at(
                                         i))) == *data->bitsets->at(j)) {
                                    std::lock_guard<std::mutex> lock(
                                        *data->mutex);
                                    data->matchingV->at(j).push_back(i);
                                }
                            }
                        }
                    },
                    &fnDataAr[i]);
            }
            threadPool->easyStartAndWait();
        }
 
        return matchingV;
    }
 
   public:
    void callForMatchingFunctions(const bool useThreadPool = false) {
        deferringDeletions.fetch_add(1);
        std::vector<BitsetType*> bitsets;
        for (auto iter = forMatchingFunctions.begin();
             iter != forMatchingFunctions.end(); ++iter) {
            bitsets.push_back(&std::get<BitsetType>(iter->second));
        }
 
        std::vector<std::vector<std::size_t> > matching =
            getMatchingEntities(bitsets, useThreadPool);
 
        std::size_t i = 0;
        for (auto iter = forMatchingFunctions.begin();
             iter != forMatchingFunctions.end(); ++iter) {
            std::get<2>(iter->second)(useThreadPool, matching[i++],
                                      std::get<1>(iter->second));
        }
 
        handleDeferredDeletions();
    }
 
    bool callForMatchingFunction(std::size_t id,
                                 const bool useThreadPool = false) {
        auto iter = forMatchingFunctions.find(id);
        if (iter == forMatchingFunctions.end()) {
            return false;
        }
        deferringDeletions.fetch_add(1);
        std::vector<std::vector<std::size_t> > matching = getMatchingEntities(
            std::vector<BitsetType*>{&std::get<BitsetType>(iter->second)},
            useThreadPool);
        std::get<2>(iter->second)(useThreadPool, matching[0],
                                  std::get<1>(iter->second));
 
        handleDeferredDeletions();
        return true;
    }
 
    void clearForMatchingFunctions() {
        forMatchingFunctions.clear();
        functionIndex = 0;
    }
 
    bool removeForMatchingFunction(std::size_t id) {
        return forMatchingFunctions.erase(id) == 1;
    }
 
    template <typename List>
    std::size_t keepSomeMatchingFunctions(List list) {
        std::size_t deletedCount = 0;
        for (auto iter = forMatchingFunctions.begin();
             iter != forMatchingFunctions.end();) {
            if (std::find(list.begin(), list.end(), iter->first) ==
                list.end()) {
                iter = forMatchingFunctions.erase(iter);
                ++deletedCount;
            } else {
                ++iter;
            }
        }
 
        return deletedCount;
    }
 
    std::size_t keepSomeMatchingFunctions(
        std::initializer_list<std::size_t> list) {
        return keepSomeMatchingFunctions<decltype(list)>(list);
    }
 
    template <typename List>
    std::size_t removeSomeMatchingFunctions(List list) {
        std::size_t deletedCount = 0;
        for (auto listIter = list.begin(); listIter != list.end(); ++listIter) {
            deletedCount += forMatchingFunctions.erase(*listIter);
        }
 
        return deletedCount;
    }
 
    std::size_t removeSomeMatchingFunctions(
        std::initializer_list<std::size_t> list) {
        return removeSomeMatchingFunctions<decltype(list)>(list);
    }
 
    bool changeForMatchingFunctionContext(std::size_t id, void* userData) {
        auto f = forMatchingFunctions.find(id);
        if (f != forMatchingFunctions.end()) {
            std::get<1>(f->second) = userData;
            return true;
        }
        return false;
    }
 
    template <typename SigList, typename FTuple>
    void forMatchingSignatures(FTuple fTuple, void* userData = nullptr,
                               const bool useThreadPool = false) {
        std::size_t current_id;
        {
            // push to idStack "call stack"
            std::lock_guard<std::mutex> lock(idStackMutex);
            current_id = idStackCounter++;
            idStack.push_back(current_id);
        }
        deferringDeletions.fetch_add(1);
        std::vector<std::vector<std::size_t> > multiMatchingEntities(
            SigList::size);
        BitsetType signatureBitsets[SigList::size];
 
        // generate bitsets for each signature
        EC::Meta::forEachWithIndex<SigList>(
            [&signatureBitsets](auto signature, const auto index) {
                signatureBitsets[index] =
                    BitsetType::template generateBitset<decltype(signature)>();
            });
 
        // find and store entities matching signatures
        if (!useThreadPool || !threadPool) {
            for (std::size_t eid = 0; eid < currentSize; ++eid) {
                if (!isAlive(eid)) {
                    continue;
                }
                for (std::size_t i = 0; i < SigList::size; ++i) {
                    if ((signatureBitsets[i] &
                         std::get<BitsetType>(entities[eid])) ==
                        signatureBitsets[i]) {
                        multiMatchingEntities[i].push_back(eid);
                    }
                }
            }
        } else {
            std::array<TPFnDataStructFour, ThreadCount * 2> fnDataAr;
 
            std::mutex mutex;
            std::size_t s = currentSize / (ThreadCount * 2);
            for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                std::size_t begin = s * i;
                std::size_t end;
                if (i == ThreadCount * 2 - 1) {
                    end = currentSize;
                } else {
                    end = s * (i + 1);
                }
                if (begin == end) {
                    continue;
                }
                fnDataAr[i].range = {begin, end};
                fnDataAr[i].manager = this;
                fnDataAr[i].multiMatchingEntities = &multiMatchingEntities;
                fnDataAr[i].signatures = signatureBitsets;
                fnDataAr[i].mutex = &mutex;
                for (std::size_t j = begin; j < end; ++j) {
                    if (!isAlive(j)) {
                        fnDataAr[i].dead.insert(j);
                    }
                }
 
                threadPool->queueFn(
                    [](void* ud) {
                        auto* data = static_cast<TPFnDataStructFour*>(ud);
                        for (std::size_t i = data->range[0]; i < data->range[1];
                             ++i) {
                            if (data->dead.find(i) != data->dead.end()) {
                                continue;
                            }
                            for (std::size_t j = 0; j < SigList::size; ++j) {
                                if ((data->signatures[j] &
                                     std::get<BitsetType>(
                                         data->manager->entities[i])) ==
                                    data->signatures[j]) {
                                    std::lock_guard<std::mutex> lock(
                                        *data->mutex);
                                    data->multiMatchingEntities->at(j)
                                        .push_back(i);
                                }
                            }
                        }
                    },
                    &fnDataAr[i]);
            }
            threadPool->easyStartAndWait();
        }
 
        // call functions on matching entities
        EC::Meta::forEachDoubleTuple(
            EC::Meta::Morph<SigList, std::tuple<> >{}, fTuple,
            [this, &multiMatchingEntities, useThreadPool, &userData](
                auto sig, auto func, auto index) {
                using SignatureComponents =
                    typename EC::Meta::Matching<decltype(sig),
                                                ComponentsList>::type;
                using Helper = EC::Meta::Morph<SignatureComponents,
                                               ForMatchingSignatureHelper<> >;
                if (!useThreadPool || !threadPool) {
                    for (const auto& id : multiMatchingEntities[index]) {
                        if (isAlive(id)) {
                            Helper::call(id, *this, func, userData);
                        }
                    }
                } else {
                    std::array<TPFnDataStructFive, ThreadCount * 2> fnDataAr;
                    std::size_t s =
                        multiMatchingEntities[index].size() / (ThreadCount * 2);
                    for (unsigned int i = 0; i < ThreadCount * 2; ++i) {
                        std::size_t begin = s * i;
                        std::size_t end;
                        if (i == ThreadCount * 2 - 1) {
                            end = multiMatchingEntities[index].size();
                        } else {
                            end = s * (i + 1);
                        }
                        if (begin == end) {
                            continue;
                        }
                        fnDataAr[i].range = {begin, end};
                        fnDataAr[i].index = index;
                        fnDataAr[i].manager = this;
                        fnDataAr[i].userData = userData;
                        fnDataAr[i].multiMatchingEntities =
                            &multiMatchingEntities;
                        for (std::size_t j = begin; j < end; ++j) {
                            if (!isAlive(
                                    multiMatchingEntities.at(index).at(j))) {
                                fnDataAr[i].dead.insert(j);
                            }
                        }
                        threadPool->queueFn(
                            [&func](void* ud) {
                                auto* data =
                                    static_cast<TPFnDataStructFive*>(ud);
                                for (std::size_t i = data->range[0];
                                     i < data->range[1]; ++i) {
                                    if (data->dead.find(i) ==
                                        data->dead.end()) {
                                        Helper::call(data->multiMatchingEntities
                                                         ->at(data->index)
                                                         .at(i),
                                                     *data->manager, func,
                                                     data->userData);
                                    }
                                }
                            },
                            &fnDataAr[i]);
                    }
                    threadPool->easyStartAndWait();
                }
            });
 
        // pop from idStack "call stack"
        do {
            {
                std::lock_guard<std::mutex> lock(idStackMutex);
                if (idStack.back() == current_id) {
                    idStack.pop_back();
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::microseconds(15));
        } while (true);
 
        handleDeferredDeletions();
    }
 
    template <typename SigList, typename FTuple>
    void forMatchingSignaturesPtr(FTuple fTuple, void* userData = nullptr,
                                  const bool useThreadPool = false) {
        std::size_t current_id;
        {
            // push to idStack "call stack"
            std::lock_guard<std::mutex> lock(idStackMutex);
            current_id = idStackCounter++;
            idStack.push_back(current_id);
        }
        deferringDeletions.fetch_add(1);
        std::vector<std::vector<std::size_t> > multiMatchingEntities(
            SigList::size);
        BitsetType signatureBitsets[SigList::size];
 
        // generate bitsets for each signature
        EC::Meta::forEachWithIndex<SigList>(
            [&signatureBitsets](auto signature, const auto index) {
                signatureBitsets[index] =
                    BitsetType::template generateBitset<decltype(signature)>();
            });
 
        // find and store entities matching signatures
        if (!useThreadPool || !threadPool) {
            for (std::size_t eid = 0; eid < currentSize; ++eid) {
                if (!isAlive(eid)) {
                    continue;
                }
                for (std::size_t i = 0; i < SigList::size; ++i) {
                    if ((signatureBitsets[i] &
                         std::get<BitsetType>(entities[eid])) ==
                        signatureBitsets[i]) {
                        multiMatchingEntities[i].push_back(eid);
                    }
                }
            }
        } else {
            std::array<TPFnDataStructSix, ThreadCount * 2> fnDataAr;
 
            std::mutex mutex;
            std::size_t s = currentSize / (ThreadCount * 2);
            for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                std::size_t begin = s * i;
                std::size_t end;
                if (i == ThreadCount * 2 - 1) {
                    end = currentSize;
                } else {
                    end = s * (i + 1);
                }
                if (begin == end) {
                    continue;
                }
                fnDataAr[i].range = {begin, end};
                fnDataAr[i].manager = this;
                fnDataAr[i].multiMatchingEntities = &multiMatchingEntities;
                fnDataAr[i].bitsets = signatureBitsets;
                fnDataAr[i].mutex = &mutex;
                for (std::size_t j = begin; j < end; ++j) {
                    if (!isAlive(j)) {
                        fnDataAr[i].dead.insert(j);
                    }
                }
 
                threadPool->queueFn(
                    [](void* ud) {
                        auto* data = static_cast<TPFnDataStructSix*>(ud);
                        for (std::size_t i = data->range[0]; i < data->range[1];
                             ++i) {
                            if (data->dead.find(i) != data->dead.end()) {
                                continue;
                            }
                            for (std::size_t j = 0; j < SigList::size; ++j) {
                                if ((data->bitsets[j] &
                                     std::get<BitsetType>(
                                         data->manager->entities[i])) ==
                                    data->bitsets[j]) {
                                    std::lock_guard<std::mutex> lock(
                                        *data->mutex);
                                    data->multiMatchingEntities->at(j)
                                        .push_back(i);
                                }
                            }
                        }
                    },
                    &fnDataAr[i]);
            }
            threadPool->easyStartAndWait();
        }
 
        // call functions on matching entities
        EC::Meta::forEachDoubleTuple(
            EC::Meta::Morph<SigList, std::tuple<> >{}, fTuple,
            [this, &multiMatchingEntities, useThreadPool, &userData](
                auto sig, auto func, auto index) {
                using SignatureComponents =
                    typename EC::Meta::Matching<decltype(sig),
                                                ComponentsList>::type;
                using Helper = EC::Meta::Morph<SignatureComponents,
                                               ForMatchingSignatureHelper<> >;
                if (!useThreadPool || !threadPool) {
                    for (const auto& id : multiMatchingEntities[index]) {
                        if (isAlive(id)) {
                            Helper::callPtr(id, *this, func, userData);
                        }
                    }
                } else {
                    std::array<TPFnDataStructFive, ThreadCount * 2> fnDataAr;
                    std::size_t s =
                        multiMatchingEntities[index].size() / (ThreadCount * 2);
                    for (unsigned int i = 0; i < ThreadCount * 2; ++i) {
                        std::size_t begin = s * i;
                        std::size_t end;
                        if (i == ThreadCount * 2 - 1) {
                            end = multiMatchingEntities[index].size();
                        } else {
                            end = s * (i + 1);
                        }
                        if (begin == end) {
                            continue;
                        }
                        fnDataAr[i].range = {begin, end};
                        fnDataAr[i].index = index;
                        fnDataAr[i].manager = this;
                        fnDataAr[i].userData = userData;
                        fnDataAr[i].multiMatchingEntities =
                            &multiMatchingEntities;
                        for (std::size_t j = begin; j < end; ++j) {
                            if (!isAlive(
                                    multiMatchingEntities.at(index).at(j))) {
                                fnDataAr[i].dead.insert(j);
                            }
                        }
                        threadPool->queueFn(
                            [&func](void* ud) {
                                auto* data =
                                    static_cast<TPFnDataStructFive*>(ud);
                                for (std::size_t i = data->range[0];
                                     i < data->range[1]; ++i) {
                                    if (data->dead.find(i) ==
                                        data->dead.end()) {
                                        Helper::callPtr(
                                            data->multiMatchingEntities
                                                ->at(data->index)
                                                .at(i),
                                            *data->manager, func,
                                            data->userData);
                                    }
                                }
                            },
                            &fnDataAr[i]);
                    }
                    threadPool->easyStartAndWait();
                }
            });
 
        // pop from idStack "call stack"
        do {
            {
                std::lock_guard<std::mutex> lock(idStackMutex);
                if (idStack.back() == current_id) {
                    idStack.pop_back();
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::microseconds(15));
        } while (true);
 
        handleDeferredDeletions();
    }
 
    typedef void ForMatchingFn(std::size_t, Manager<ComponentsList, TagsList>*,
                               void*);
 
    template <typename Signature>
    void forMatchingSimple(ForMatchingFn fn, void* userData = nullptr,
                           const bool useThreadPool = false) {
        std::size_t current_id;
        {
            // push to idStack "call stack"
            std::lock_guard<std::mutex> lock(idStackMutex);
            current_id = idStackCounter++;
            idStack.push_back(current_id);
        }
        deferringDeletions.fetch_add(1);
        const BitsetType signatureBitset =
            BitsetType::template generateBitset<Signature>();
        if (!useThreadPool || !threadPool) {
            for (std::size_t i = 0; i < currentSize; ++i) {
                if (!std::get<bool>(entities[i])) {
                    continue;
                } else if ((signatureBitset &
                            std::get<BitsetType>(entities[i])) ==
                           signatureBitset) {
                    fn(i, this, userData);
                }
            }
        } else {
            std::array<TPFnDataStructZero, ThreadCount * 2> fnDataAr;
 
            std::size_t s = currentSize / (ThreadCount * 2);
            for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                std::size_t begin = s * i;
                std::size_t end;
                if (i == ThreadCount * 2 - 1) {
                    end = currentSize;
                } else {
                    end = s * (i + 1);
                }
                if (begin == end) {
                    continue;
                }
                fnDataAr[i].range = {begin, end};
                fnDataAr[i].manager = this;
                fnDataAr[i].entities = &entities;
                fnDataAr[i].signature = &signatureBitset;
                fnDataAr[i].userData = userData;
                for (std::size_t j = begin; j < end; ++j) {
                    if (!isAlive(j)) {
                        fnDataAr[i].dead.insert(j);
                    }
                }
                threadPool->queueFn(
                    [&fn](void* ud) {
                        auto* data = static_cast<TPFnDataStructZero*>(ud);
                        for (std::size_t i = data->range[0]; i < data->range[1];
                             ++i) {
                            if (data->dead.find(i) != data->dead.end()) {
                                continue;
                            } else if ((*data->signature &
                                        std::get<BitsetType>(data->entities->at(
                                            i))) == *data->signature) {
                                fn(i, data->manager, data->userData);
                            }
                        }
                    },
                    &fnDataAr[i]);
            }
            threadPool->easyStartAndWait();
        }
 
        // pop from idStack "call stack"
        do {
            {
                std::lock_guard<std::mutex> lock(idStackMutex);
                if (idStack.back() == current_id) {
                    idStack.pop_back();
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::microseconds(15));
        } while (true);
 
        handleDeferredDeletions();
    }
 
    template <typename Iterable>
    void forMatchingIterable(Iterable iterable, ForMatchingFn fn,
                             void* userData = nullptr,
                             const bool useThreadPool = false) {
        std::size_t current_id;
        {
            // push to idStack "call stack"
            std::lock_guard<std::mutex> lock(idStackMutex);
            current_id = idStackCounter++;
            idStack.push_back(current_id);
        }
 
        deferringDeletions.fetch_add(1);
        if (!useThreadPool || !threadPool) {
            bool isValid;
            for (std::size_t i = 0; i < currentSize; ++i) {
                if (!std::get<bool>(entities[i])) {
                    continue;
                }
 
                isValid = true;
                for (const auto& integralValue : iterable) {
                    if (!std::get<BitsetType>(entities[i])
                             .getCombinedBit(integralValue)) {
                        isValid = false;
                        break;
                    }
                }
                if (!isValid) {
                    continue;
                }
 
                fn(i, this, userData);
            }
        } else {
            std::array<TPFnDataStructSeven<Iterable>, ThreadCount * 2> fnDataAr;
 
            std::size_t s = currentSize / (ThreadCount * 2);
            for (std::size_t i = 0; i < ThreadCount * 2; ++i) {
                std::size_t begin = s * i;
                std::size_t end;
                if (i == ThreadCount * 2 - 1) {
                    end = currentSize;
                } else {
                    end = s * (i + 1);
                }
                if (begin == end) {
                    continue;
                }
                fnDataAr[i].range = {begin, end};
                fnDataAr[i].manager = this;
                fnDataAr[i].entities = &entities;
                fnDataAr[i].iterable = &iterable;
                fnDataAr[i].userData = userData;
                for (std::size_t j = begin; j < end; ++j) {
                    if (!isAlive(j)) {
                        fnDataAr[i].dead.insert(j);
                    }
                }
                threadPool->queueFn(
                    [&fn](void* ud) {
                        auto* data =
                            static_cast<TPFnDataStructSeven<Iterable>*>(ud);
                        bool isValid;
                        for (std::size_t i = data->range[0]; i < data->range[1];
                             ++i) {
                            if (data->dead.find(i) != data->dead.end()) {
                                continue;
                            }
                            isValid = true;
                            for (const auto& integralValue : *data->iterable) {
                                if (!std::get<BitsetType>(data->entities->at(i))
                                         .getCombinedBit(integralValue)) {
                                    isValid = false;
                                    break;
                                }
                            }
                            if (!isValid) {
                                continue;
                            }
 
                            fn(i, data->manager, data->userData);
                        }
                    },
                    &fnDataAr[i]);
            }
            threadPool->easyStartAndWait();
        }
 
        // pop from idStack "call stack"
        do {
            {
                std::lock_guard<std::mutex> lock(idStackMutex);
                if (idStack.back() == current_id) {
                    idStack.pop_back();
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::microseconds(15));
        } while (true);
 
        handleDeferredDeletions();
    }
};
}  // namespace EC
 
#endif