Multiset<>

libuavcan/include/uavcan/util/map.hpp

@@ -137,7 +137,10 @@ protected:
 #endif
 
     /// Derived class destructor must call removeAll();
-    ~MapBase() { }
+    ~MapBase()
+    {
+        UAVCAN_ASSERT(getSize() == 0);
+    }
 
 public:
     /**
@@ -158,7 +161,7 @@ public:
     /**
      * Removes entries where the predicate returns true.
      * Predicate prototype:
-     *  bool (const Key& key, const Value& value)
+     *  bool (Key& key, Value& value)
      */
     template <typename Predicate>
     void removeWhere(Predicate predicate);

libuavcan/include/uavcan/util/multiset.hpp

@@ -0,0 +1,570 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED
+#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED
+
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/dynamic_memory.hpp>
+#include <uavcan/util/templates.hpp>
+
+namespace uavcan
+{
+/**
+ * Slow but memory efficient unordered set.
+ *
+ * Items can be allocated in a static buffer or in the node's memory pool if the static buffer is exhausted.
+ * When an item is deleted from the static buffer, one pair from the memory pool will be moved in the free
+ * slot of the static buffer, so the use of the memory pool is minimized.
+ *
+ * Please be aware that this container does not perform any speed optimizations to minimize memory footprint,
+ * so the complexity of most operations is O(N).
+ *
+ * Type requirements:
+ *  T must be copyable, assignable and default constructible.
+ *  T must implement a comparison operator.
+ *  T's default constructor must initialize the object into invalid state.
+ *  Size of T must not exceed MemPoolBlockSize.
+ */
+template <typename T>
+class UAVCAN_EXPORT MultisetBase : Noncopyable
+{
+    template <typename, unsigned> friend class Multiset;
+
+protected:
+    /*
+     * Purpose of this type is to enforce default initialization of T
+     */
+    struct Item
+    {
+        T value;
+        Item() : value() { }
+        Item(const T& v) : value(v) { }
+        bool operator==(const Item& rhs) const { return rhs.value == value; }
+        bool operator!=(const Item& rhs) const { return !operator==(rhs); }
+        operator T() const { return value; }
+    };
+
+    struct Chunk : LinkedListNode<Chunk>
+    {
+        enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode<Chunk>)) / sizeof(Item) };
+        Item items[NumItems];
+
+        Chunk()
+        {
+            StaticAssert<(static_cast<unsigned>(NumItems) > 0)>::check();
+            IsDynamicallyAllocatable<Chunk>::check();
+            UAVCAN_ASSERT(items[0].value == T());
+        }
+
+        static Chunk* instantiate(IPoolAllocator& allocator)
+        {
+            void* const praw = allocator.allocate(sizeof(Chunk));
+            if (praw == NULL)
+            {
+                return NULL;
+            }
+            return new (praw) Chunk();
+        }
+
+        static void destroy(Chunk*& obj, IPoolAllocator& allocator)
+        {
+            if (obj != NULL)
+            {
+                obj->~Chunk();
+                allocator.deallocate(obj);
+                obj = NULL;
+            }
+        }
+
+        Item* find(const Item& item)
+        {
+            for (unsigned i = 0; i < static_cast<unsigned>(NumItems); i++)
+            {
+                if (items[i] == item)
+                {
+                    return items + i;
+                }
+            }
+            return NULL;
+        }
+    };
+
+private:
+    LinkedListRoot<Chunk> list_;
+    IPoolAllocator& allocator_;
+#if !UAVCAN_TINY
+    Item* const static_;
+    const unsigned num_static_entries_;
+#endif
+
+    Item* find(const Item& item);
+
+#if !UAVCAN_TINY
+    void optimizeStorage();
+#endif
+    void compact();
+
+    struct YesPredicate
+    {
+        bool operator()(const T&) const { return true; }
+    };
+
+protected:
+#if UAVCAN_TINY
+    MultisetBase(IPoolAllocator& allocator)
+        : allocator_(allocator)
+    {
+        UAVCAN_ASSERT(Item() == Item());
+    }
+#else
+    MultisetBase(Item* static_buf, unsigned num_static_entries, IPoolAllocator& allocator)
+        : allocator_(allocator)
+        , static_(static_buf)
+        , num_static_entries_(num_static_entries)
+    {
+        UAVCAN_ASSERT(Item() == Item());
+    }
+#endif
+
+    /// Derived class destructor must call removeAll();
+    ~MultisetBase()
+    {
+        UAVCAN_ASSERT(getSize() == 0);
+    }
+
+public:
+    /**
+     * Adds one item and returns a pointer to it.
+     * If add fails due to lack of memory, NULL will be returned.
+     */
+    T* add(const T& item);
+
+    /**
+     * Does nothing if there's no such item.
+     */
+    void remove(const T& item);
+
+    /**
+     * Removes entries where the predicate returns true.
+     * Predicate prototype:
+     *  bool (T& item)
+     */
+    template <typename Predicate>
+    void removeWhere(Predicate predicate);
+
+    /**
+     * Returns first entry where the predicate returns true.
+     * Predicate prototype:
+     *  bool (const T& item)
+     */
+    template <typename Predicate>
+    const T* findFirst(Predicate predicate) const;
+
+    /**
+     * Removes all items; all pool memory will be released.
+     */
+    void removeAll();
+
+    /**
+     * Returns an item located at the specified position from the beginning.
+     * Note that any insertion or deletion may greatly disturb internal ordering, so use with care.
+     * If index is greater than or equal the number of items, null pointer will be returned.
+     */
+    T* getByIndex(unsigned index);
+    const T* getByIndex(unsigned index) const;
+
+    bool isEmpty() const;
+
+    /**
+     * Counts number of items stored.
+     * Best case complexity is O(N).
+     */
+    unsigned getSize() const;
+
+    /**
+     * For testing, do not use directly.
+     */
+    unsigned getNumStaticItems() const;
+    unsigned getNumDynamicItems() const;
+};
+
+
+template <typename T, unsigned NumStaticEntries = 0>
+class UAVCAN_EXPORT Multiset : public MultisetBase<T>
+{
+    typename MultisetBase<T>::Item static_[NumStaticEntries];
+
+public:
+
+#if !UAVCAN_TINY
+
+    // This instantiation will not be valid in UAVCAN_TINY mode
+    explicit Multiset(IPoolAllocator& allocator)
+        : MultisetBase<T>(static_, NumStaticEntries, allocator)
+    {
+        UAVCAN_ASSERT(static_[0].value == T());
+    }
+
+    ~Multiset() { this->removeAll(); }
+
+#endif // !UAVCAN_TINY
+};
+
+
+template <typename T>
+class UAVCAN_EXPORT Multiset<T, 0> : public MultisetBase<T>
+{
+public:
+    explicit Multiset(IPoolAllocator& allocator)
+#if UAVCAN_TINY
+        : MultisetBase<T>(allocator)
+#else
+        : MultisetBase<T>(NULL, 0, allocator)
+#endif
+    { }
+
+    ~Multiset() { this->removeAll(); }
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * MultisetBase<>
+ */
+template <typename T>
+typename MultisetBase<T>::Item* MultisetBase<T>::find(const Item& item)
+{
+#if !UAVCAN_TINY
+    for (unsigned i = 0; i < num_static_entries_; i++)
+    {
+        if (static_[i] == item)
+        {
+            return static_ + i;
+        }
+    }
+#endif
+
+    Chunk* p = list_.get();
+    while (p)
+    {
+        Item* const dyn = p->find(item);
+        if (dyn != NULL)
+        {
+            return dyn;
+        }
+        p = p->getNextListNode();
+    }
+    return NULL;
+}
+
+#if !UAVCAN_TINY
+
+template <typename T>
+void MultisetBase<T>::optimizeStorage()
+{
+    while (true)
+    {
+        // Looking for first EMPTY static entry
+        Item* stat = NULL;
+        for (unsigned i = 0; i < num_static_entries_; i++)
+        {
+            if (static_[i] == Item())
+            {
+                stat = static_ + i;
+                break;
+            }
+        }
+        if (stat == NULL)
+        {
+            break;
+        }
+
+        // Looking for the first NON-EMPTY dynamic entry, erasing immediately
+        Chunk* p = list_.get();
+        Item dyn;
+        UAVCAN_ASSERT(dyn == Item());
+        while (p)
+        {
+            bool stop = false;
+            for (int i = 0; i < Chunk::NumItems; i++)
+            {
+                if (p->items[i] != Item())   // Non empty
+                {
+                    dyn = p->items[i];       // Copy by value
+                    p->items[i] = Item();    // Erase immediately
+                    stop = true;
+                    break;
+                }
+            }
+            if (stop)
+            {
+                break;
+            }
+            p = p->getNextListNode();
+        }
+        if (dyn == Item())
+        {
+            break;
+        }
+
+        // Migrating
+        *stat = dyn;
+    }
+}
+
+#endif // !UAVCAN_TINY
+
+template <typename T>
+void MultisetBase<T>::compact()
+{
+    Chunk* p = list_.get();
+    while (p)
+    {
+        Chunk* const next = p->getNextListNode();
+        bool remove_this = true;
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            if (p->items[i] != Item())
+            {
+                remove_this = false;
+                break;
+            }
+        }
+        if (remove_this)
+        {
+            list_.remove(p);
+            Chunk::destroy(p, allocator_);
+        }
+        p = next;
+    }
+}
+
+template <typename T>
+T* MultisetBase<T>::add(const T& value)
+{
+    UAVCAN_ASSERT(!(value == T()));
+    remove(value);
+
+    Item* const item = find(Item());
+    if (item)
+    {
+        *item = Item(value);
+        return &item->value;
+    }
+
+    Chunk* const itemg = Chunk::instantiate(allocator_);
+    if (itemg == NULL)
+    {
+        return NULL;
+    }
+    list_.insert(itemg);
+    itemg->items[0] = Item(value);
+    return &itemg->items[0].value;
+}
+
+template <typename T>
+void MultisetBase<T>::remove(const T& value)
+{
+    UAVCAN_ASSERT(!(value == T()));
+    Item* const item = find(Item(value));
+    if (item != NULL)
+    {
+        *item = Item();
+#if !UAVCAN_TINY
+        optimizeStorage();
+#endif
+        compact();
+    }
+}
+
+template <typename T>
+template <typename Predicate>
+void MultisetBase<T>::removeWhere(Predicate predicate)
+{
+    unsigned num_removed = 0;
+
+#if !UAVCAN_TINY
+    for (unsigned i = 0; i < num_static_entries_; i++)
+    {
+        if (static_[i] != Item())
+        {
+            if (predicate(static_[i].value))
+            {
+                num_removed++;
+                static_[i] = Item();
+            }
+        }
+    }
+#endif
+
+    Chunk* p = list_.get();
+    while (p)
+    {
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            const Item* const item = p->items + i;
+            if ((*item) != Item())
+            {
+                if (predicate(item->value))
+                {
+                    num_removed++;
+                    p->items[i] = Item();
+                }
+            }
+        }
+        p = p->getNextListNode();
+    }
+
+    if (num_removed > 0)
+    {
+#if !UAVCAN_TINY
+        optimizeStorage();
+#endif
+        compact();
+    }
+}
+
+template <typename T>
+template <typename Predicate>
+const T* MultisetBase<T>::findFirst(Predicate predicate) const
+{
+#if !UAVCAN_TINY
+    for (unsigned i = 0; i < num_static_entries_; i++)
+    {
+        if (static_[i] != Item())
+        {
+            if (predicate(static_[i].value))
+            {
+                return &static_[i].value;
+            }
+        }
+    }
+#endif
+
+    Chunk* p = list_.get();
+    while (p)
+    {
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            const Item* const item = p->items + i;
+            if ((*item) != Item())
+            {
+                if (predicate(item->value))
+                {
+                    return &p->items[i].value;
+                }
+            }
+        }
+        p = p->getNextListNode();
+    }
+    return NULL;
+}
+
+template <typename T>
+void MultisetBase<T>::removeAll()
+{
+    removeWhere(YesPredicate());
+}
+
+template <typename T>
+T* MultisetBase<T>::getByIndex(unsigned index)
+{
+#if !UAVCAN_TINY
+    // Checking the static storage
+    for (unsigned i = 0; i < num_static_entries_; i++)
+    {
+        if (static_[i] != Item())
+        {
+            if (index == 0)
+            {
+                return &static_[i].value;
+            }
+            index--;
+        }
+    }
+#endif
+
+    // Slowly crawling through the dynamic storage
+    Chunk* p = list_.get();
+    while (p)
+    {
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            Item* const item = p->items + i;
+            if ((*item) != Item())
+            {
+                if (index == 0)
+                {
+                    return &item->value;
+                }
+                index--;
+            }
+        }
+        p = p->getNextListNode();
+    }
+
+    return NULL;
+}
+
+template <typename T>
+const T* MultisetBase<T>::getByIndex(unsigned index) const
+{
+    return const_cast<MultisetBase<T>*>(this)->getByIndex(index);
+}
+
+template <typename T>
+bool MultisetBase<T>::isEmpty() const
+{
+    return getSize() == 0;
+}
+
+template <typename T>
+unsigned MultisetBase<T>::getSize() const
+{
+    return getNumStaticItems() + getNumDynamicItems();
+}
+
+template <typename T>
+unsigned MultisetBase<T>::getNumStaticItems() const
+{
+    unsigned num = 0;
+#if !UAVCAN_TINY
+    for (unsigned i = 0; i < num_static_entries_; i++)
+    {
+        if (static_[i] != Item())
+        {
+            num++;
+        }
+    }
+#endif
+    return num;
+}
+
+template <typename T>
+unsigned MultisetBase<T>::getNumDynamicItems() const
+{
+    unsigned num = 0;
+    Chunk* p = list_.get();
+    while (p)
+    {
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            const Item* const item = p->items + i;
+            if ((*item) != Item())
+            {
+                num++;
+            }
+        }
+        p = p->getNextListNode();
+    }
+    return num;
+}
+
+}
+
+#endif // Include guard

libuavcan/test/util/multiset.cpp

@@ -0,0 +1,225 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <string>
+#include <cstdio>
+#include <memory>
+#include <gtest/gtest.h>
+#include <uavcan/util/multiset.hpp>
+
+
+static std::string toString(long x)
+{
+    char buf[80];
+    std::snprintf(buf, sizeof(buf), "%li", x);
+    return std::string(buf);
+}
+
+static bool oddValuePredicate(const std::string& value)
+{
+    EXPECT_FALSE(value.empty());
+    const int num = atoi(value.c_str());
+    return num & 1;
+}
+
+struct FindPredicate
+{
+    const std::string target;
+    FindPredicate(const std::string& target) : target(target) { }
+    bool operator()(const std::string& value) const { return value == target; }
+};
+
+
+TEST(Multiset, Basic)
+{
+    using uavcan::Multiset;
+
+    static const int POOL_BLOCKS = 3;
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    typedef Multiset<std::string, 2> MultisetType;
+    std::auto_ptr<MultisetType> mset(new MultisetType(poolmgr));
+
+    // Empty
+    mset->remove("foo");
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_FALSE(mset->getByIndex(0));
+    ASSERT_FALSE(mset->getByIndex(1));
+    ASSERT_FALSE(mset->getByIndex(10000));
+
+    // Static addion
+    ASSERT_EQ("1", *mset->add("1"));
+    ASSERT_EQ("2", *mset->add("2"));
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    ASSERT_EQ(0, mset->getNumDynamicItems());
+
+    // Ordering
+    ASSERT_TRUE(*mset->getByIndex(0) == "1");
+    ASSERT_TRUE(*mset->getByIndex(1) == "2");
+
+    // Dynamic addion
+    ASSERT_EQ("3", *mset->add("3"));
+    ASSERT_EQ(1, pool.getNumUsedBlocks());
+
+    ASSERT_EQ("4", *mset->add("4"));
+    ASSERT_EQ(1, pool.getNumUsedBlocks());       // Assuming that at least 2 items fit one block
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    ASSERT_EQ(2, mset->getNumDynamicItems());
+
+    // Making sure everything is here
+    ASSERT_EQ("1", *mset->getByIndex(0));
+    ASSERT_EQ("2", *mset->getByIndex(1));
+    ASSERT_EQ("3", *mset->getByIndex(2));
+    ASSERT_EQ("4", *mset->getByIndex(3));
+    ASSERT_FALSE(mset->getByIndex(100));
+    ASSERT_FALSE(mset->getByIndex(4));
+
+    // Finding some items
+    ASSERT_EQ("1", *mset->findFirst(FindPredicate("1")));
+    ASSERT_EQ("2", *mset->findFirst(FindPredicate("2")));
+    ASSERT_EQ("3", *mset->findFirst(FindPredicate("3")));
+    ASSERT_EQ("4", *mset->findFirst(FindPredicate("4")));
+    ASSERT_FALSE(mset->findFirst(FindPredicate("nonexistent")));
+
+    // Removing one static
+    mset->remove("1");                             // One of dynamics now migrates to the static storage
+    mset->remove("foo");                           // There's no such thing anyway
+    ASSERT_EQ(1, pool.getNumUsedBlocks());
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    ASSERT_EQ(1, mset->getNumDynamicItems());
+
+    // Ordering has not changed - first dynamic entry has moved to the first static slot
+    ASSERT_EQ("3", *mset->getByIndex(0));
+    ASSERT_EQ("2", *mset->getByIndex(1));
+    ASSERT_EQ("4", *mset->getByIndex(2));
+
+    // Removing another static
+    mset->remove("2");
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    ASSERT_EQ(0, mset->getNumDynamicItems());
+    ASSERT_EQ(0, pool.getNumUsedBlocks());       // No dynamic entries left
+
+    // Adding some new dynamics
+    unsigned max_value_integer = 0;
+    for (int i = 0; i < 100; i++)
+    {
+        const std::string value = toString(i);
+        std::string* res = mset->add(value);  // Will override some from the above
+        if (res == NULL)
+        {
+            ASSERT_LT(2, i);
+            break;
+        }
+        else
+        {
+            ASSERT_EQ(value, *res);
+        }
+        max_value_integer = unsigned(i);
+    }
+    std::cout << "Max value: " << max_value_integer << std::endl;
+    ASSERT_LT(4, max_value_integer);
+
+    // Making sure there is true OOM
+    ASSERT_EQ(0, pool.getNumFreeBlocks());
+    ASSERT_FALSE(mset->add("nonexistent"));
+
+    // Removing odd values - nearly half of them
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    const unsigned num_dynamics_old = mset->getNumDynamicItems();
+    mset->removeWhere(oddValuePredicate);
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    const unsigned num_dynamics_new = mset->getNumDynamicItems();
+    std::cout << "Num of dynamic pairs reduced from " << num_dynamics_old << " to " << num_dynamics_new << std::endl;
+    ASSERT_LT(num_dynamics_new, num_dynamics_old);
+
+    // Making sure there's no odd values left
+    for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++)
+    {
+        const std::string* val = mset->findFirst(FindPredicate(toString(kv_int)));
+        if (val)
+        {
+            ASSERT_FALSE(kv_int & 1);
+        }
+        else
+        {
+            ASSERT_TRUE(kv_int & 1);
+        }
+    }
+
+    // Making sure the memory will be released
+    mset.reset();
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+}
+
+
+TEST(Multiset, NoStatic)
+{
+    using uavcan::Multiset;
+
+    static const int POOL_BLOCKS = 3;
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    typedef Multiset<std::string> MultisetType;
+    std::auto_ptr<MultisetType> mset(new MultisetType(poolmgr));
+
+    // Empty
+    mset->remove("foo");
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_FALSE(mset->getByIndex(0));
+
+    // Insertion
+    ASSERT_EQ("a", *mset->add("a"));
+    ASSERT_EQ("b", *mset->add("b"));
+    ASSERT_EQ(1, pool.getNumUsedBlocks());
+    ASSERT_EQ(0, mset->getNumStaticItems());
+    ASSERT_EQ(2, mset->getNumDynamicItems());
+
+    // Ordering
+    ASSERT_EQ("a", *mset->getByIndex(0));
+    ASSERT_EQ("b", *mset->getByIndex(1));
+    ASSERT_FALSE(mset->getByIndex(3));
+    ASSERT_FALSE(mset->getByIndex(1000));
+}
+
+
+TEST(Multiset, PrimitiveKey)
+{
+    using uavcan::Multiset;
+
+    static const int POOL_BLOCKS = 3;
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    typedef Multiset<short, 2> MultisetType;
+    std::auto_ptr<MultisetType> mset(new MultisetType(poolmgr));
+
+    // Empty
+    mset->remove(8);
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_EQ(0, mset->getSize());
+    ASSERT_FALSE(mset->getByIndex(0));
+
+    // Insertion
+    ASSERT_EQ(1, *mset->add(1));
+    ASSERT_EQ(1, mset->getSize());
+    ASSERT_EQ(2, *mset->add(2));
+    ASSERT_EQ(2, mset->getSize());
+    ASSERT_EQ(3, *mset->add(3));
+    ASSERT_EQ(4, *mset->add(4));
+    ASSERT_EQ(4, mset->getSize());
+
+    // Ordering
+    ASSERT_EQ(1, *mset->getByIndex(0));
+    ASSERT_EQ(2, *mset->getByIndex(1));
+    ASSERT_EQ(3, *mset->getByIndex(2));
+    ASSERT_EQ(4, *mset->getByIndex(3));
+    ASSERT_FALSE(mset->getByIndex(5));
+    ASSERT_FALSE(mset->getByIndex(1000));
+}