Improve SB addNotification

src/state_block.cpp

@@ -29,6 +29,36 @@ void StateBlock::setFixed(bool _fixed)
         getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_FIX);
 }
 
+void StateBlock::addNotification(const StateBlock::Notification _new_notification)
+{
+    std::lock_guard<std::mutex> lock(notifictions_mut_);
+    if (_new_notification == Notification::ADD)
+    {
+        // When an ADD arrives, the state is already the newest,
+        // thus old instructions can be cleared
+        if (shared_from_this()->notifications_.size() > 0)
+            notifications_.clear();
+
+        // Push ADD notification to the front
+        notifications_.emplace_front(Notification::ADD);
+    }
+    else if (_new_notification == Notification::REMOVE)
+    {
+        // If we want to remove a block that still has an ADD instruction
+        // we can just clear all notifications and just keep the remove
+        if (!notifications_.empty() && notifications_.front() == Notification::ADD)
+            notifications_.clear();
+        else
+        {
+            notifications_.clear();
+            notifications_.emplace_back(Notification::REMOVE);
+        }
+    }
+    else
+        // UPDATE_FIX; UPDATE_STATE
+        notifications_.emplace_back(_new_notification);
+}
+
 StateBlock::Notifications StateBlock::consumeNotifications() const
 {
     std::lock_guard<std::mutex> lock(notifictions_mut_);

src/state_block.h

@@ -34,7 +34,7 @@ public:
 
   enum class Notification : std::size_t
   {
-    ADD = 0,
+    ADD = 1,
     REMOVE,
     UPDATE_STATE,
     UPDATE_FIX
@@ -239,12 +239,6 @@ inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _
     local_param_ptr_ = _local_param;
 }
 
-inline void StateBlock::addNotification(const StateBlock::Notification _new_notification)
-{
-  std::lock_guard<std::mutex> lock(notifictions_mut_);
-  notifications_.emplace_back(_new_notification);
-}
-
 inline void StateBlock::setProblem(const ProblemPtr _problem)
 {
     problem_ptr_ = _problem;