diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d035fbd84..62b8efaad 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -78,6 +78,7 @@ IF (MQTT_TEST_7)
         as_buffer_pubsub.cpp
         as_buffer_async_pubsub_1.cpp
         as_buffer_async_pubsub_2.cpp
+        subscription_map.cpp
         subscription_map_broker.cpp
         async_pubsub_2.cpp
     )
diff --git a/test/retained_topic_map.hpp b/test/retained_topic_map.hpp
index 098e0f621..ab98d1205 100644
--- a/test/retained_topic_map.hpp
+++ b/test/retained_topic_map.hpp
@@ -60,7 +60,7 @@ class retained_topic_map {
 
                 if (entry == map.end()) {
                     entry = map.emplace(path_entry_key(parent_id, MQTT_NS::allocate_buffer(t)), path_entry(next_node_id++)).first;
-                    if (next_node_id == std::numeric_limits<typeof(next_node_id)>::max()) {
+                    if (next_node_id == std::numeric_limits<node_id_t>::max()) {
                         throw std::overflow_error("Maximum number of topics reached");
                     }
                 }
@@ -189,7 +189,7 @@ class retained_topic_map {
     }
 
     // Remove a value at the specified subscription path
-    bool remove_topic(MQTT_NS::string_view topic) {
+    bool erase_topic(MQTT_NS::string_view topic) {
         auto path = find_topic(topic);
         if (path.empty()) {
             return false;
@@ -230,13 +230,7 @@ class retained_topic_map {
 
     // Insert a value at the specified subscription path
     void insert_or_update(MQTT_NS::string_view topic, Value const& value) {
-        auto path = find_topic(topic);
-        if (path.empty()) {
-            this->create_topic(topic)->second.value = value;
-        }
-        else {
-            path.back()->second.value = value;
-        }
+        this->create_topic(topic)->second.value = value;
     }
 
     // Find all stored topics that math the specified subscription
@@ -245,9 +239,13 @@ class retained_topic_map {
     }
 
     // Remove a stored value at the specified topic
-    void remove(MQTT_NS::string_view topic) {
-        remove_topic(topic);
+    size_t erase(MQTT_NS::string_view topic) {
+        return (erase_topic(topic) ? 1 : 0);
     }
+
+    // Get the size of the map
+    size_t size() const { return map.size(); }
+
 };
 
 #endif // MQTT_RETAINED_TOPIC_MAP_HPP
diff --git a/test/subscription_map.cpp b/test/subscription_map.cpp
new file mode 100644
index 000000000..c11505173
--- /dev/null
+++ b/test/subscription_map.cpp
@@ -0,0 +1,284 @@
+// Copyright Takatoshi Kondo 2020
+//
+// Distributed under the Boost Software License, Version 1.0.
+// (See accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+#include "test_main.hpp"
+#include "combi_test.hpp"
+#include "checker.hpp"
+#include "global_fixture.hpp"
+
+#include "subscription_map.hpp"
+#include "retained_topic_map.hpp"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_SUITE(test_subscription_map)
+
+BOOST_AUTO_TEST_CASE( failed_erase ) {
+    using func_t = std::function<void()>;
+    using value_t = std::shared_ptr<func_t>; // shared_ptr for '<' and hash
+    using sm_t = multiple_subscription_map<value_t>;
+
+    sm_t m;
+    auto v1 = std::make_shared<func_t>([] { std::cout << "v1" << std::endl; });
+    auto v2 = std::make_shared<func_t>([] { std::cout << "v2" << std::endl; });
+
+    BOOST_TEST(m.size() == 1);
+    auto it_success1 = m.insert("a/b/c", v1);
+    assert(it_success1.second);
+    BOOST_TEST(m.size() != 1);
+
+    auto it_success2 = m.insert("a/b", v2);
+    assert(it_success2.second);
+    BOOST_TEST(m.size() != 1);
+
+    auto e1 = m.erase(it_success1.first, v1);
+    BOOST_TEST(m.size() != 1);
+
+    auto e2 = m.erase(it_success2.first, v2); //  Invalid handle was specified is thrown here
+    BOOST_TEST(m.size() == 1);
+
+}	
+
+BOOST_AUTO_TEST_CASE( TestSingleSubscription ) {
+    std::string text = "example/test/A";
+
+    single_subscription_map< std::string > map;
+    auto handle = map.insert(text, text);
+    BOOST_TEST(handle.size() == 3);
+    BOOST_TEST(map.handle_to_subscription(handle) == text);
+    BOOST_CHECK_THROW(map.insert(text, text), std::exception);
+    map.update(handle, "new_value");
+    map.erase(handle);
+
+    BOOST_TEST(map.size() == 1);
+
+    map.insert(text, text);
+    BOOST_TEST(map.size() != 1);
+
+    map.erase(text);
+    BOOST_TEST(map.size() == 1);
+
+    std::vector<std::string> values = {
+        "example/test/A", "example/+/A", "example/#", "#"
+    };
+
+    for(auto const &i: values) {
+        map.insert(i, i);
+    }
+
+    // Attempt to remove entry which has no value
+    BOOST_TEST(map.erase("example") == 0);
+    BOOST_TEST(map.erase(map.lookup("example")) == 0);
+    BOOST_TEST(map.erase("example") == 0);
+    BOOST_TEST(map.erase(map.lookup("example")) == 0);
+
+    std::vector<std::string> matches;
+    map.find("example/test/A", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 4);
+
+    matches = {};
+    map.find("hash_match_only", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 1);
+
+    matches = {};
+    map.find("example/hash_only", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 2);
+
+    matches = {};
+    map.find("example/plus/A", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 3);
+
+    BOOST_TEST(map.erase("non-existent") == 0);
+
+    for(auto const &i: values) {
+        BOOST_TEST(map.size() != 0);
+        BOOST_TEST(map.erase(i) == 1);
+    }
+
+    BOOST_TEST(map.size() == 1);
+
+    std::vector< single_subscription_map< std::string >::handle > handles;
+    for(auto const &i: values) {
+        handles.push_back(map.insert(i, i));
+    }
+
+    for(auto const &i: handles) {
+        BOOST_TEST(map.size() != 0);
+        BOOST_TEST(map.erase(i) == 1);
+    }
+
+    BOOST_TEST(map.size() == 1);
+
+}
+
+BOOST_AUTO_TEST_CASE( TestMultipleSubscription ) {
+    std::string text = "example/test/A";
+
+    multiple_subscription_map<std::string> map;
+
+    map.insert("a/b/c", "123");
+    map.insert("a/b", "123");
+
+    map.erase("a/b/c", "123");
+    BOOST_TEST(map.size() != 1);
+
+    map.erase("a/b", "123");
+    BOOST_TEST(map.size() == 1);
+
+    std::vector<std::string> values = {
+        "example/test/A", "example/+/A", "example/#", "#"
+    };
+
+    // Add some duplicates and overlapping paths
+    map.insert(values[0], values[0]);
+    BOOST_TEST(map.insert(values[0], values[0]).second == false);
+    BOOST_TEST(map.insert(values[0], "blaat").second == true);
+
+    map.erase(values[0], "blaat");
+    BOOST_TEST(map.size() != 1);
+
+    map.erase(values[0], values[0]);
+    BOOST_TEST(map.size() == 1);
+
+    // Perform test again but this time using handles
+    map.insert(values[0], values[0]);
+    BOOST_TEST(map.insert(map.lookup(values[0]), values[0]).second == false);
+    BOOST_TEST(map.insert(map.lookup(values[0]), "blaat").second == true);
+
+    map.erase(map.lookup(values[0]), "blaat");
+    BOOST_TEST(map.size() != 1);
+
+    map.erase(map.lookup(values[0]), values[0]);
+    BOOST_TEST(map.size() == 1);
+
+    for(auto const &i: values) {
+        map.insert(i, i);
+    }
+
+    // Attempt to remove entry which has no value
+    BOOST_TEST(map.erase("example", "example") == 0);
+    BOOST_TEST(map.erase(map.lookup("example"), "example") == 0);
+    BOOST_TEST(map.erase("example", "example") == 0);
+    BOOST_TEST(map.erase(map.lookup("example"), "example") == 0);
+
+    BOOST_TEST(map.lookup(values[0]).size() == 3);
+    BOOST_TEST(map.handle_to_subscription(map.lookup(values[0])) == values[0]);
+
+    std::vector<std::string> matches;
+    map.find("example/test/A", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 4);
+
+    matches = {};
+    map.find("hash_match_only", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 1);
+
+    matches = {};
+    map.find("example/hash_only", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 2);
+
+    matches = {};
+    map.find("example/plus/A", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 3);
+
+    BOOST_TEST(map.erase("non-existent", "non-existent") == 0);
+
+    for(auto const &i: values) {
+        BOOST_TEST(map.size() != 0);
+        BOOST_TEST(map.erase(i, i) == 1);
+    }
+
+    BOOST_TEST(map.size() == 1);
+}
+
+
+
+BOOST_AUTO_TEST_CASE(retained_topic_map_test) {
+    retained_topic_map<std::string> map;
+    map.insert_or_update("a/b/c", "123");
+    map.insert_or_update("a/b", "123");
+
+    map.erase("a/b/c");
+    BOOST_TEST(map.size() != 1);
+
+    map.erase("a/b");
+    BOOST_TEST(map.size() == 1);
+
+    std::vector<std::string> values = {
+        "example/test/A", "example/test/B", "example/A/test", "example/B/test"
+    };
+
+    for(auto const &i: values) {
+        map.insert_or_update(i, i);
+    }
+
+    std::vector<std::string> matches;
+    map.find(values[0], [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 1);
+    BOOST_TEST(matches[0] == values[0]);
+
+    matches = { };
+    map.find(values[1], [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 1);
+    BOOST_TEST(matches[0] == values[1]);
+
+    matches = { };
+    map.find("example/test/+", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 2);
+    BOOST_TEST(matches[0] == values[0]);
+    BOOST_TEST(matches[1] == values[1]);
+
+    matches = { };
+    map.find("example/+/B", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+    BOOST_TEST(matches.size() == 1);
+    BOOST_TEST(matches[0] == values[1]);
+
+    matches = { };
+    map.find("example/#", [&matches](std::string const &a) {
+        matches.push_back(a);
+    });
+
+    BOOST_TEST(matches.size() == 4);
+
+    std::vector<std::string> diff;
+    std::sort(matches.begin(), matches.end());
+    std::sort(values.begin(), values.end());
+    std::set_difference(matches.begin(), matches.end(), values.begin(), values.end(), diff.begin());
+    BOOST_TEST(diff.empty());
+
+    BOOST_TEST(map.erase("non-existent") == 0);
+
+    for(auto const &i: values) {
+        BOOST_TEST(map.size() != 0);
+        BOOST_TEST(map.erase(i) == 1);
+    }
+
+    BOOST_TEST(map.size() == 1);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/test/subscription_map.hpp b/test/subscription_map.hpp
index d719c6f67..4f87e7b31 100644
--- a/test/subscription_map.hpp
+++ b/test/subscription_map.hpp
@@ -263,6 +263,14 @@ class subscription_map_base {
         return result;
     }
 
+    // Increase the number of subscriptions for this handle
+    void increase_subscriptions(handle h) {
+        std::vector<map_type_iterator> iterators = handle_to_iterators(h);
+        for (auto i : iterators) {
+            ++(i->second.count);
+        }
+    }
+
     subscription_map_base()
         : next_node_id(root_node_id)
     {
@@ -300,7 +308,7 @@ class subscription_map_base {
 
 template<typename Value>
 class single_subscription_map
-    : public subscription_map_base<Value> {
+    : public subscription_map_base< boost::optional<Value> > {
 
 public:
 
@@ -310,8 +318,12 @@ class single_subscription_map
     // Insert a value at the specified subscription path
     handle insert(MQTT_NS::string_view subscription, Value value) {
         auto existing_subscription = this->find_subscription(subscription);
-        if (!existing_subscription.empty())
-            throw std::runtime_error("Subscription already exists in map");
+        if (!existing_subscription.empty()) {
+            if(existing_subscription.back()->second.value)
+                throw std::runtime_error("Subscription already exists in map");
+            existing_subscription.back()->second.value = std::move(value);
+            return this->path_to_handle(existing_subscription);
+        }
 
         auto new_subscription_path = this->create_subscription(subscription);
         new_subscription_path.back()->second.value = std::move(value);
@@ -337,28 +349,48 @@ class single_subscription_map
     }
 
     // Remove a value at the specified subscription path
-    void erase(MQTT_NS::string_view subscription) {
+    size_t erase(MQTT_NS::string_view subscription) {
         auto path = this->find_subscription(subscription);
         if (path.empty()) {
-            return;
+            return 0;
+        }
+
+        if(!path.back()->second.value) {
+            return 0;
         }
 
         this->remove_subscription(path);
+        return 1;
     }
 
     // Remove a value using a handle
-    void erase(handle h) {
+    size_t erase(handle h) {
         auto path = this->handle_to_iterators(h);
-        if (!path.empty()) {
-            this->remove_subscription(path);
+        if (path.empty()) {
+            return 0;
         }
+
+        if(!path.back()->second.value) {
+            return 0;
+        }
+
+        this->remove_subscription(path);
+        return 1;
     }
 
     // Find all subscriptions that match the specified topic
     template<typename Output>
     void find(MQTT_NS::string_view topic, Output callback) const {
-        this->find_match(topic, std::move(callback));
+        this->find_match(
+            topic,
+            [&callback]( boost::optional<Value> value ) {
+                if(value) {
+                    callback(value.get());
+                }
+            }
+        );
     }
+
 };
 
 
@@ -374,15 +406,16 @@ class multiple_subscription_map
     // Insert a value at the specified subscription path
     std::pair<handle, bool> insert(MQTT_NS::string_view subscription, Value value) {
         auto path = this->find_subscription(subscription);
-        if (path.empty()) {
+        if(path.empty()) {
             auto new_subscription_path = this->create_subscription(subscription);
             new_subscription_path.back()->second.value.insert(std::move(value));
             return std::make_pair(this->path_to_handle(new_subscription_path), true);
+        } else {
+            auto result = path.back()->second.value.insert(std::move(value));
+            if(result.second)
+                this->create_subscription(subscription);
+            return std::make_pair(this->path_to_handle(path), result.second);
         }
-
-        auto &subscription_set = path.back()->second.value;
-        bool insert_result = subscription_set.insert(std::move(value)).second;
-        return std::make_pair(this->path_to_handle(path), insert_result);
     }
 
     // Insert a value with a handle to the subscription
@@ -398,8 +431,10 @@ class multiple_subscription_map
         }
 
         auto& subscription_set = h_iter->second.value;
-        bool insert_result = subscription_set.insert(std::move(value)).second;
-        return std::make_pair(h, insert_result);
+        auto insert_result = subscription_set.insert(std::move(value));
+        if(insert_result.second)
+            this->increase_subscriptions(h);
+        return std::make_pair(h, insert_result.second);
     }
 
     // Remove a value at the specified subscription path
@@ -408,15 +443,13 @@ class multiple_subscription_map
         // Find the subscription in the map
         auto path = this->find_subscription(subscription);
         if (path.empty()) {
-            return boost::optional<Value>();
+            return 0;
         }
 
         // Remove the specified value
         auto& subscription_set = path.back()->second.value;
         auto result = subscription_set.erase(value);
-
-        // If all values removed, remove the subscription
-        if(subscription_set.empty())
+        if(result)
             this->remove_subscription(path);
 
         return result;
@@ -438,15 +471,13 @@ class multiple_subscription_map
         // Remove the specified value
         auto& subscription_set = h_iter->second.value;
         auto result = subscription_set.erase(value);
-
-        // If all values removed, remove the subscription
-        if(subscription_set.empty())
+        if(result)
             this->remove_subscription(this->handle_to_iterators(h));
 
         return result;
     }
 
-        // Find all subscriptions that match the specified topic
+    // Find all subscriptions that match the specified topic
     template<typename Output>
     void find(MQTT_NS::string_view topic, Output callback) const {
         this->find_match(