Use StateBlockComposite in HasStateBlocks

include/core/state_block/has_state_blocks.h

@@ -21,15 +21,15 @@ class HasStateBlocks
 {
     public:
         HasStateBlocks();
-        HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
+        HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_composite_() {}
         virtual ~HasStateBlocks();
 
         const StateStructure& getStructure() const { return structure_; }
         void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
         bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
 
-        const StateBlockMap& getStateBlockMap() const;
-        std::vector<StateBlockPtr> getStateBlockVec() const;
+        const StateBlockMap&        getStateBlockMap() const;
+        std::vector<StateBlockPtr>  getStateBlockVec() const;
 
         // Some typical shortcuts -- not all should be coded here, see notes below.
         StateBlockPtr getP() const;
@@ -44,7 +44,7 @@ class HasStateBlocks
         virtual StateBlockPtr   addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); }
         virtual unsigned int    removeStateBlock(const std::string& _sb_type);
         virtual unsigned int    removeStateBlock(const char _sb_type);
-        bool                    hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+        bool                    hasStateBlock(const std::string& _sb_type) const { return state_block_composite_.has(_sb_type); }
         bool                    hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
         bool                    hasStateBlock(const StateBlockPtr& _sb) const;
         StateBlockPtr           getStateBlock(const std::string& _sb_type) const;
@@ -77,6 +77,9 @@ class HasStateBlocks
         bool            getVectorComposite(VectorComposite& _state) const;
         void            setVectorComposite(const VectorComposite& _state);
 
+        StateBlockComposite&        getStateBlockComposite();
+        const StateBlockComposite&  getStateBlockComposite() const;
+
         // Perturb state with noise
         void            perturb(double amplitude = 0.01);
 
@@ -85,8 +88,8 @@ class HasStateBlocks
 
 
     private:
-        StateStructure  structure_; // TBR
-        StateBlockMap   state_block_map_;
+        StateStructure      structure_;             // TODO: remove?
+        StateBlockComposite state_block_composite_;
 
 };
 
@@ -101,7 +104,8 @@ namespace wolf{
 
 inline HasStateBlocks::HasStateBlocks() :
         structure_(),
-        state_block_map_()
+//        state_block_map_(),
+        state_block_composite_()
 {
     //
 }
@@ -113,7 +117,7 @@ inline HasStateBlocks::~HasStateBlocks()
 
 inline const StateBlockMap& HasStateBlocks::getStateBlockMap() const
 {
-    return state_block_map_;
+    return state_block_composite_.getStateBlockMap();
 }
 
 inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
@@ -133,13 +137,15 @@ inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
 
 inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type)
 {
-    return state_block_map_.erase(_sb_type);
+    return state_block_composite_.remove(_sb_type);
 }
 
 template<typename SB, typename ... Args>
 inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
 {
-    assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+    assert(state_block_composite_.has(_sb_type) == false && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+//    assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
     std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
 
     addStateBlock(_sb_type, sb, _problem);
@@ -150,7 +156,8 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string&
 template<typename ... Args>
 inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
 {
-    assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+    assert(state_block_composite_.has(_sb_type) == false && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+//    assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
     auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
 
     addStateBlock(_sb_type, sb, _problem);
@@ -161,18 +168,14 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty
 inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb)
 {
     assert (structure_.find(_sb_type) != std::string::npos && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
-    assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
-    state_block_map_.at(_sb_type) = _sb;
+    assert ( state_block_composite_.has(_sb_type)  && "Cannot set an inexistent state block! Use addStateBlock instead.");
+    state_block_composite_.set(_sb_type, _sb);
     return true; // success
 }
 
 inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const
 {
-    auto it = state_block_map_.find(_sb_type);
-    if (it != state_block_map_.end())
-        return it->second;
-    else
-        return nullptr;
+    return state_block_composite_.at(_sb_type);
 }
 
 inline wolf::StateBlockPtr HasStateBlocks::getP() const
@@ -187,27 +190,17 @@ inline wolf::StateBlockPtr HasStateBlocks::getO() const
 
 inline void HasStateBlocks::fix()
 {
-    for (auto pair_key_sbp : state_block_map_)
-        if (pair_key_sbp.second != nullptr)
-            pair_key_sbp.second->fix();
+    state_block_composite_.fix();
 }
 
 inline void HasStateBlocks::unfix()
 {
-    for (auto pair_key_sbp : state_block_map_)
-        if (pair_key_sbp.second != nullptr)
-            pair_key_sbp.second->unfix();
+    state_block_composite_.unfix();
 }
 
 inline bool HasStateBlocks::isFixed() const
 {
-    bool fixed = true;
-    for (auto pair_key_sbp : state_block_map_)
-    {
-        if (pair_key_sbp.second)
-            fixed &= pair_key_sbp.second->isFixed();
-    }
-    return fixed;
+    return state_block_composite_.isFixed();
 }
 
 inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
@@ -268,40 +261,17 @@ inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_struc
 
 inline StateBlockMap::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
 {
-    const auto& it = std::find_if(state_block_map_.begin(),
-                                  state_block_map_.end(),
-                                  [_sb](const std::pair<std::string, StateBlockPtr>& pair)
-                                  {
-                                    return pair.second == _sb;
-                                  }
-                                  );
-
-    return it;
+    return state_block_composite_.find(_sb);
 }
 
 inline bool HasStateBlocks::hasStateBlock(const StateBlockPtr &_sb) const
 {
-    const auto& it = this->find(_sb);
-
-    return it != state_block_map_.end();
+    return state_block_composite_.has(_sb);
 }
 
 inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string& _key) const
 {
-    const auto& it = this->find(_sb);
-
-    bool found = (it != state_block_map_.end());
-
-    if (found)
-    {
-        _key = it->first;
-        return true;
-    }
-    else
-    {
-        _key = "";
-        return false;
-    }
+    return state_block_composite_.key(_sb, _key);
 }
 
 inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
@@ -344,6 +314,16 @@ inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_stru
     return size;
 }
 
+inline StateBlockComposite& HasStateBlocks::getStateBlockComposite()
+{
+    return state_block_composite_;
+}
+
+inline const StateBlockComposite& HasStateBlocks::getStateBlockComposite() const
+{
+    return state_block_composite_;
+}
+
 } // namespace wolf
 
 #endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */

src/state_block/has_state_blocks.cpp

@@ -6,8 +6,12 @@ namespace wolf
 
 StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
 {
-    assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
-    state_block_map_.emplace(_sb_type, _sb);
+    assert(state_block_composite_.has(_sb) == false && "Trying to add a state block that already exists!");
+
+    assert(state_block_composite_.has(_sb_type) == false && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+    state_block_composite_.add(_sb_type, _sb);
+
     if (!isInStructure(_sb_type))
         appendToStructure(_sb_type);
 
@@ -46,7 +50,7 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
 
 bool HasStateBlocks::getVectorComposite(VectorComposite &_state) const
 {
-    for (auto &pair_key_sb : state_block_map_)
+    for (auto &pair_key_sb : state_block_composite_.getStateBlockMap())
     {
         _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
     }
@@ -64,13 +68,16 @@ void HasStateBlocks::setVectorComposite(const VectorComposite &_state)
 {
     for (const auto &pair_key_sb : _state)
     {
-        state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
+        const auto& sb = state_block_composite_.at(pair_key_sb.first);
+
+        if(sb)
+            sb->setState(pair_key_sb.second);
     }
 }
 
 void HasStateBlocks::perturb(double amplitude)
 {
-    for (const auto& pair_key_sb : state_block_map_)
+    for (const auto& pair_key_sb : state_block_composite_.getStateBlockMap())
     {
         auto& sb = pair_key_sb.second;
         if (!sb->isFixed())
@@ -80,7 +87,7 @@ void HasStateBlocks::perturb(double amplitude)
 
 void HasStateBlocks::plus(const VectorComposite& _dx)
 {
-    for (const auto& pair_key_sb : state_block_map_)
+    for (const auto& pair_key_sb : state_block_composite_.getStateBlockMap())
     {
         auto& sb = pair_key_sb.second;
         auto& dv = _dx.at(pair_key_sb.first);