From b5e201647f007c4a178a0c0c9f241395346a49f1 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Tue, 18 Mar 2014 14:58:05 +0400
Subject: [PATCH] Map<> got the new method findFirstKey<>(), which is needed
 for the upcoming Dispatcher logic

---
 libuavcan/include/uavcan/map.hpp | 33 ++++++++++++++++++++++++++++++++
 libuavcan/test/map.cpp           | 28 +++++++++++++++++++++++++++
 2 files changed, 61 insertions(+)

diff --git a/libuavcan/include/uavcan/map.hpp b/libuavcan/include/uavcan/map.hpp
index e6b1b3977c..277df04bb9 100644
--- a/libuavcan/include/uavcan/map.hpp
+++ b/libuavcan/include/uavcan/map.hpp
@@ -268,6 +268,39 @@ public:
         }
     }
 
+    template <typename Predicate>
+    const Key* findFirstKey(Predicate predicate) const
+    {
+        for (unsigned int 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;
+    }
+
     void removeAll()
     {
         removeWhere(YesPredicate());
diff --git a/libuavcan/test/map.cpp b/libuavcan/test/map.cpp
index 33b39ca983..4840eeca9e 100644
--- a/libuavcan/test/map.cpp
+++ b/libuavcan/test/map.cpp
@@ -24,6 +24,20 @@ static bool oddValuePredicate(const std::string& key, const std::string& value)
     return num & 1;
 }
 
+struct KeyFindPredicate
+{
+    const std::string target;
+    KeyFindPredicate(std::string target) : target(target) { }
+    bool operator()(const std::string& key, const std::string&) const { return key == target; }
+};
+
+struct ValueFindPredicate
+{
+    const std::string target;
+    ValueFindPredicate(std::string target) : target(target) { }
+    bool operator()(const std::string&, const std::string& value) const { return value == target; }
+};
+
 
 TEST(Map, Basic)
 {
@@ -75,6 +89,20 @@ TEST(Map, Basic)
     ASSERT_EQ("C", *map->access("3"));
     ASSERT_EQ("D", *map->access("4"));
 
+    // Finding some keys
+    ASSERT_EQ("1", *map->findFirstKey(KeyFindPredicate("1")));
+    ASSERT_EQ("2", *map->findFirstKey(KeyFindPredicate("2")));
+    ASSERT_EQ("3", *map->findFirstKey(KeyFindPredicate("3")));
+    ASSERT_EQ("4", *map->findFirstKey(KeyFindPredicate("4")));
+    ASSERT_FALSE(map->findFirstKey(KeyFindPredicate("nonexistent_key")));
+
+    // Finding some values
+    ASSERT_EQ("1", *map->findFirstKey(ValueFindPredicate("A")));
+    ASSERT_EQ("2", *map->findFirstKey(ValueFindPredicate("B")));
+    ASSERT_EQ("3", *map->findFirstKey(ValueFindPredicate("C")));
+    ASSERT_EQ("4", *map->findFirstKey(ValueFindPredicate("D")));
+    ASSERT_FALSE(map->findFirstKey(KeyFindPredicate("nonexistent_value")));
+
     // Removing one static
     map->remove("1");                             // One of dynamics now migrates to the static storage
     map->remove("foo");                           // There's no such thing anyway