From a13e4de58a21da8ad80a5efce07dde37787ecc85 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Tue, 8 Apr 2014 18:43:40 +0400
Subject: [PATCH] Much space optimized Map<> container - saves 40kb of Flash
 for STM32 test (-O1)

---
 libuavcan/include/uavcan/map.hpp | 478 +++++++++++++++++--------------
 1 file changed, 262 insertions(+), 216 deletions(-)

diff --git a/libuavcan/include/uavcan/map.hpp b/libuavcan/include/uavcan/map.hpp
index 4c4d4777c9..fe929678d7 100644
--- a/libuavcan/include/uavcan/map.hpp
+++ b/libuavcan/include/uavcan/map.hpp
@@ -86,103 +86,11 @@ class UAVCAN_EXPORT Map : Noncopyable
     IAllocator& allocator_;
     KVPair static_[NumStaticEntries];
 
-    KVPair* find(const Key& key)
-    {
-        for (unsigned i = 0; i < NumStaticEntries; i++)
-        {
-            if (static_[i].match(key))
-            {
-                return static_ + i;
-            }
-        }
+    KVPair* find(const Key& key);
 
-        KVGroup* p = list_.get();
-        while (p)
-        {
-            KVPair* const kv = p->find(key);
-            if (kv)
-            {
-                return kv;
-            }
-            p = p->getNextListNode();
-        }
-        return NULL;
-    }
+    void optimizeStorage();
 
-    void optimizeStorage()
-    {
-        while (true)
-        {
-            // Looking for first EMPTY static entry
-            KVPair* stat = NULL;
-            for (unsigned i = 0; i < NumStaticEntries; i++)
-            {
-                if (static_[i].match(Key()))
-                {
-                    stat = static_ + i;
-                    break;
-                }
-            }
-            if (stat == NULL)
-            {
-                break;
-            }
-
-            // Looking for the first NON-EMPTY dynamic entry, erasing immediately
-            KVGroup* p = list_.get();
-            KVPair dyn;
-            while (p)
-            {
-                bool stop = false;
-                for (int i = 0; i < KVGroup::NumKV; i++)
-                {
-                    if (!p->kvs[i].match(Key())) // Non empty
-                    {
-                        dyn = p->kvs[i];         // Copy by value
-                        p->kvs[i] = KVPair();    // Erase immediately
-                        stop = true;
-                        break;
-                    }
-                }
-                if (stop)
-                {
-                    break;
-                }
-                p = p->getNextListNode();
-            }
-            if (dyn.match(Key()))
-            {
-                break;
-            }
-
-            // Migrating
-            *stat = dyn;
-        }
-    }
-
-    void compact()
-    {
-        KVGroup* p = list_.get();
-        while (p)
-        {
-            KVGroup* const next = p->getNextListNode();
-            bool remove_this = true;
-            for (int i = 0; i < KVGroup::NumKV; i++)
-            {
-                if (!p->kvs[i].match(Key()))
-                {
-                    remove_this = false;
-                    break;
-                }
-            }
-            if (remove_this)
-            {
-                list_.remove(p);
-                KVGroup::destroy(p, allocator_);
-            }
-            p = next;
-        }
-    }
+    void compact();
 
     struct YesPredicate
     {
@@ -202,47 +110,12 @@ public:
 
     ~Map() { removeAll(); }
 
-    Value* access(const Key& key)
-    {
-        assert(!(key == Key()));
-        KVPair* const kv = find(key);
-        return kv ? &kv->value : NULL;
-    }
+    Value* access(const Key& key);
 
     /// If entry with the same key already exists, it will be replaced
-    Value* insert(const Key& key, const Value& value)
-    {
-        assert(!(key == Key()));
-        remove(key);
+    Value* insert(const Key& key, const Value& value);
 
-        KVPair* const kv = find(Key());
-        if (kv)
-        {
-            *kv = KVPair(key, value);
-            return &kv->value;
-        }
-
-        KVGroup* const kvg = KVGroup::instantiate(allocator_);
-        if (kvg == NULL)
-        {
-            return NULL;
-        }
-        list_.insert(kvg);
-        kvg->kvs[0] = KVPair(key, value);
-        return &kvg->kvs[0].value;
-    }
-
-    void remove(const Key& key)
-    {
-        assert(!(key == Key()));
-        KVPair* const kv = find(key);
-        if (kv)
-        {
-            *kv = KVPair();
-            optimizeStorage();
-            compact();
-        }
-    }
+    void remove(const Key& key);
 
     /**
      * Remove entries where predicate returns true.
@@ -250,120 +123,293 @@ public:
      *  bool (const Key& key, const Value& value)
      */
     template <typename Predicate>
-    void removeWhere(Predicate predicate)
-    {
-        unsigned num_removed = 0;
-
-        for (unsigned i = 0; i < NumStaticEntries; i++)
-        {
-            if (!static_[i].match(Key()))
-            {
-                if (predicate(static_[i].key, static_[i].value))
-                {
-                    num_removed++;
-                    static_[i] = KVPair();
-                }
-            }
-        }
-
-        KVGroup* p = list_.get();
-        while (p)
-        {
-            for (int i = 0; i < KVGroup::NumKV; i++)
-            {
-                const KVPair* const kv = p->kvs + i;
-                if (!kv->match(Key()))
-                {
-                    if (predicate(kv->key, kv->value))
-                    {
-                        num_removed++;
-                        p->kvs[i] = KVPair();
-                    }
-                }
-            }
-            p = p->getNextListNode();
-        }
-
-        if (num_removed > 0)
-        {
-            optimizeStorage();
-            compact();
-        }
-    }
+    void removeWhere(Predicate predicate);
 
     template <typename Predicate>
-    const Key* findFirstKey(Predicate predicate) const
+    const Key* findFirstKey(Predicate predicate) const;
+
+    void removeAll();
+
+    bool isEmpty() const;
+
+    /// For testing
+    unsigned getNumStaticPairs() const;
+
+    /// For testing
+    unsigned getNumDynamicPairs() const;
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * Map<>
+ */
+template <typename Key, typename Value, unsigned NumStaticEntries>
+typename Map<Key, Value, NumStaticEntries>::KVPair* Map<Key, Value, NumStaticEntries>::find(const Key& key)
+{
+    for (unsigned i = 0; i < NumStaticEntries; i++)
     {
+        if (static_[i].match(key))
+        {
+            return static_ + i;
+        }
+    }
+
+    KVGroup* p = list_.get();
+    while (p)
+    {
+        KVPair* const kv = p->find(key);
+        if (kv)
+        {
+            return kv;
+        }
+        p = p->getNextListNode();
+    }
+    return NULL;
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+void Map<Key, Value, NumStaticEntries>::optimizeStorage()
+{
+    while (true)
+    {
+        // Looking for first EMPTY static entry
+        KVPair* stat = NULL;
         for (unsigned i = 0; i < NumStaticEntries; i++)
         {
-            if (!static_[i].match(Key()))
+            if (static_[i].match(Key()))
             {
-                if (predicate(static_[i].key, static_[i].value))
-                {
-                    return &static_[i].key;
-                }
+                stat = static_ + i;
+                break;
             }
         }
+        if (stat == NULL)
+        {
+            break;
+        }
 
+        // Looking for the first NON-EMPTY dynamic entry, erasing immediately
         KVGroup* p = list_.get();
+        KVPair dyn;
         while (p)
         {
+            bool stop = false;
             for (int i = 0; i < KVGroup::NumKV; i++)
             {
-                const KVPair* const kv = p->kvs + i;
-                if (!kv->match(Key()))
+                if (!p->kvs[i].match(Key())) // Non empty
                 {
-                    if (predicate(kv->key, kv->value))
-                    {
-                        return &p->kvs[i].key;
-                    }
+                    dyn = p->kvs[i];         // Copy by value
+                    p->kvs[i] = KVPair();    // Erase immediately
+                    stop = true;
+                    break;
                 }
             }
+            if (stop)
+            {
+                break;
+            }
             p = p->getNextListNode();
         }
+        if (dyn.match(Key()))
+        {
+            break;
+        }
+
+        // Migrating
+        *stat = dyn;
+    }
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+void Map<Key, Value, NumStaticEntries>::compact()
+{
+    KVGroup* p = list_.get();
+    while (p)
+    {
+        KVGroup* const next = p->getNextListNode();
+        bool remove_this = true;
+        for (int i = 0; i < KVGroup::NumKV; i++)
+        {
+            if (!p->kvs[i].match(Key()))
+            {
+                remove_this = false;
+                break;
+            }
+        }
+        if (remove_this)
+        {
+            list_.remove(p);
+            KVGroup::destroy(p, allocator_);
+        }
+        p = next;
+    }
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+Value* Map<Key, Value, NumStaticEntries>::access(const Key& key)
+{
+    assert(!(key == Key()));
+    KVPair* const kv = find(key);
+    return kv ? &kv->value : NULL;
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+Value* Map<Key, Value, NumStaticEntries>::insert(const Key& key, const Value& value)
+{
+    assert(!(key == Key()));
+    remove(key);
+
+    KVPair* const kv = find(Key());
+    if (kv)
+    {
+        *kv = KVPair(key, value);
+        return &kv->value;
+    }
+
+    KVGroup* const kvg = KVGroup::instantiate(allocator_);
+    if (kvg == NULL)
+    {
         return NULL;
     }
+    list_.insert(kvg);
+    kvg->kvs[0] = KVPair(key, value);
+    return &kvg->kvs[0].value;
+}
 
-    void removeAll()
+template <typename Key, typename Value, unsigned NumStaticEntries>
+void Map<Key, Value, NumStaticEntries>::remove(const Key& key)
+{
+    assert(!(key == Key()));
+    KVPair* const kv = find(key);
+    if (kv)
     {
-        removeWhere(YesPredicate());
+        *kv = KVPair();
+        optimizeStorage();
+        compact();
+    }
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+template <typename Predicate>
+void Map<Key, Value, NumStaticEntries>::removeWhere(Predicate predicate)
+{
+    unsigned num_removed = 0;
+
+    for (unsigned i = 0; i < NumStaticEntries; i++)
+    {
+        if (!static_[i].match(Key()))
+        {
+            if (predicate(static_[i].key, static_[i].value))
+            {
+                num_removed++;
+                static_[i] = KVPair();
+            }
+        }
     }
 
-    bool isEmpty() const { return (getNumStaticPairs() == 0) && (getNumDynamicPairs() == 0); }
-
-    /// For testing
-    unsigned getNumStaticPairs() const
+    KVGroup* p = list_.get();
+    while (p)
     {
-        unsigned num = 0;
-        for (unsigned i = 0; i < NumStaticEntries; i++)
+        for (int i = 0; i < KVGroup::NumKV; i++)
         {
-            if (!static_[i].match(Key()))
+            const KVPair* const kv = p->kvs + i;
+            if (!kv->match(Key()))
+            {
+                if (predicate(kv->key, kv->value))
+                {
+                    num_removed++;
+                    p->kvs[i] = KVPair();
+                }
+            }
+        }
+        p = p->getNextListNode();
+    }
+
+    if (num_removed > 0)
+    {
+        optimizeStorage();
+        compact();
+    }
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+template <typename Predicate>
+const Key* Map<Key, Value, NumStaticEntries>::findFirstKey(Predicate predicate) const
+{
+    for (unsigned i = 0; i < NumStaticEntries; i++)
+    {
+        if (!static_[i].match(Key()))
+        {
+            if (predicate(static_[i].key, static_[i].value))
+            {
+                return &static_[i].key;
+            }
+        }
+    }
+
+    KVGroup* p = list_.get();
+    while (p)
+    {
+        for (int i = 0; i < KVGroup::NumKV; i++)
+        {
+            const KVPair* const kv = p->kvs + i;
+            if (!kv->match(Key()))
+            {
+                if (predicate(kv->key, kv->value))
+                {
+                    return &p->kvs[i].key;
+                }
+            }
+        }
+        p = p->getNextListNode();
+    }
+    return NULL;
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+void Map<Key, Value, NumStaticEntries>::removeAll()
+{
+    removeWhere(YesPredicate());
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+bool Map<Key, Value, NumStaticEntries>::isEmpty() const
+{
+    return (getNumStaticPairs() == 0) && (getNumDynamicPairs() == 0);
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+unsigned Map<Key, Value, NumStaticEntries>::getNumStaticPairs() const
+{
+    unsigned num = 0;
+    for (unsigned i = 0; i < NumStaticEntries; i++)
+    {
+        if (!static_[i].match(Key()))
+        {
+            num++;
+        }
+    }
+    return num;
+}
+
+template <typename Key, typename Value, unsigned NumStaticEntries>
+unsigned Map<Key, Value, NumStaticEntries>::getNumDynamicPairs() const
+{
+    unsigned num = 0;
+    KVGroup* p = list_.get();
+    while (p)
+    {
+        for (int i = 0; i < KVGroup::NumKV; i++)
+        {
+            const KVPair* const kv = p->kvs + i;
+            if (!kv->match(Key()))
             {
                 num++;
             }
         }
-        return num;
+        p = p->getNextListNode();
     }
-
-    /// For testing
-    unsigned getNumDynamicPairs() const
-    {
-        unsigned num = 0;
-        KVGroup* p = list_.get();
-        while (p)
-        {
-            for (int i = 0; i < KVGroup::NumKV; i++)
-            {
-                const KVPair* const kv = p->kvs + i;
-                if (!kv->match(Key()))
-                {
-                    num++;
-                }
-            }
-            p = p->getNextListNode();
-        }
-        return num;
-    }
-};
+    return num;
+}
 
 }