diff --git a/include/intervalmap.hpp b/include/intervalmap.hpp
index 6f7222b..7b20055 100644
--- a/include/intervalmap.hpp
+++ b/include/intervalmap.hpp
@@ -38,8 +38,9 @@ public:
 	    if(end_next_entry->first != end)
 	    treemap[end] = end_prev_entry->second;
 
+	    auto e = treemap.upper_bound(end);
 	    // A little optimization: delete next key if it is of the same value of the end key
-	    //if(end_next_entry->second == treemap[end]) treemap.erase(end_next_entry);
+	    if(e != treemap.end() && e->second == treemap[end]) treemap.erase(end_next_entry);
         }
 
         // insert the start key. Replaces whatever value is already there. Do not place if the element before is of the same value