diff --git a/src/include/interval-map/interval-map.hh b/src/include/interval-map/interval-map.hh
index a514daf..ab3fb44 100644
--- a/src/include/interval-map/interval-map.hh
+++ b/src/include/interval-map/interval-map.hh
@@ -15,26 +15,45 @@ public:
         if (!(begin < end))
             return;
 
-        auto const end_val = (*this)[end];
-        underlying_.erase(underlying_.lower_bound(begin),
-                          underlying_.upper_bound(end));
-        if (!((*this)[begin] == val))
-            underlying_.insert({begin, val});
-        if (!((*this)[end] == end_val))
-            underlying_.insert({end, end_val});
+        auto it = underlying_.upper_bound(end);
+        auto const end_val = at_upper_bound(it);
+
+        bool insert_begin = !(val == init_);
+
+        while (it != underlying_.begin()) {
+            it = std::prev(it);
+            auto begin_found = it->first < begin;
+            if (begin_found) {
+                insert_begin = !(val == it->second);
+                break;
+            }
+            if (it != underlying_.end())
+                // Account for up-coming `std::prev` with `++`
+                underlying_.erase(it++);
+        }
+
+        if (insert_begin)
+            it = underlying_.insert(it, {begin, val});
+        // Get the proper upper-bound for `end`
+        it = (it == underlying_.end()) ? it : std::next(it);
+        if (!(at_upper_bound(it) == end_val))
+            underlying_.insert(it, {end, end_val});
     }
 
     V const& operator[](K const& key) const {
-        auto it = underlying_.upper_bound(key);
-        if (it == underlying_.begin())
-            return init_;
-        return std::prev(it)->second;
+        return at_upper_bound(underlying_.upper_bound(key));
     }
 
     // Used in testing
     friend class ::IntervalMapTest;
 
 private:
+    V const& at_upper_bound(std::map<K, V>::const_iterator it) const {
+        if (it == underlying_.begin())
+            return init_;
+        return std::prev(it)->second;
+    }
+
     V init_;
     std::map<K, V> underlying_{};
 };