BeliefState.hh

Go to the documentation of this file.

#ifndef __DIADES__AUTOMATA__EXPERIMENTAL__BELIEFSTATE__HH
#define __DIADES__AUTOMATA__EXPERIMENTAL__BELIEFSTATE__HH
#include<stack>
#include<diades/graph/ConstEdgeMap.hh>
#include<diades/graph/ConstNodeMap.hh>
#include<diades/graph/NodeMap.hh>
#include<diades/graph/GraphNodeSet.hh>
#include<diades/automata/experimental/StateMachine.hh>

namespace Diades
{
  namespace Automata
  {
    namespace Experimental
    {

      template<typename StateMachine>
      class BeliefState : public Diades::Graph::GraphNodeSet
      {
      public:
        using Gns = Diades::Graph::GraphNodeSet;
        using Fsm = StateMachine;
        using State = typename Fsm::State;
        using Transition = typename Fsm::Transition;
        using EventPropertyId = typename Fsm::EventPropertyId;
        using SharedPtr = std::shared_ptr<BeliefState>;
        //using Iterator = typename std::set<State>::const_iterator; 
      private:
        const Fsm & _fsm;
        //std::set<State> _states;

      public:

        BeliefState(const Fsm & fsm) : Gns(fsm.behaviour()), _fsm(fsm)
        {
        }

                                
        void
        insertState(State s)
        {
          insertNode(s);
        }

                                
        template<class InputStateIterator>
        void
        insertStates(InputStateIterator first, InputStateIterator last)
        {
          insertNodes(first, last);
        }


        void removeState(State s)
        {
          removeNode(s);
        }

        Iterator
        findState(State s) const
        {
          return findNode(s);
        }



        
        template<typename IsGoal>
        SharedPtr
        nextPredicateBeliefState(IsGoal isGoal)
        {
          SharedPtr result = std::make_shared<BeliefState>(_fsm);
          for(auto state : _nodes) //nodes)
            {
              std::for_each(_fsm.outputTransitionBegin(state),
                            _fsm.outputTransitionEnd(state),
                            [&](typename Fsm::Transition trans)
                            {
                              if(isGoal(trans))
                                {
                                  result->insertState(trans.target());
                                }
                            });
            }
          return result;
        }


        template<typename IsGoal>
        SharedPtr
        prevPredicateBeliefState(IsGoal isGoal)
        {
          SharedPtr result = std::make_shared<BeliefState>(_fsm);
          for(auto state : _nodes) //_nodes)
            {
              std::for_each(_fsm.inputTransitionBegin(state),
                            _fsm.inputTransitionEnd(state),
                            [&](typename Fsm::Transition trans)
                            {
                              if(isGoal(trans))
                                {
                                  result->insertState(trans.source());
                                }
                            });
            }
          return result;
        }
        
        SharedPtr
            nextBeliefState(const EventPropertyId & event) const
          {
            SharedPtr result = std::make_shared<BeliefState>(_fsm);
            for(auto state : _nodes) //_nodes)
              {
                std::for_each(_fsm.outputEventTransitionBegin(state,event),
                              _fsm.outputEventTransitionEnd(state,event),
                              [&](typename Fsm::Transition trans)
                              {
                                result->insertState(trans.target());
                              });
              }
            return result;
          }

          SharedPtr
            prevBeliefState(const EventPropertyId & event) const
          {
            SharedPtr result = std::make_shared<BeliefState>(_fsm);
            for(auto state : _nodes) //_nodes)
              {
                std::for_each(_fsm.inputEventTransitionBegin(state,event),
                              _fsm.inputEventTransitionEnd(state,event),
                              [&](typename Fsm::Transition trans)
                              {
                                result->insertState(trans.source());
                              });
              }
            return result;
          }


                          
          SharedPtr
            nextBeliefState() const
          {
            SharedPtr result = std::make_shared<BeliefState>(_fsm);
            for(auto state : _nodes)//_nodes)
              {
                std::for_each(_fsm.outputTransitionBegin(state),
                        _fsm.outputTransitionEnd(state),
                        [&](typename Fsm::Transition trans)
                        {
                          result->insertState(trans.target());
                        });
              }
            return result;
          }

        SharedPtr
        prevBeliefState() const
        {
          SharedPtr result = std::make_shared<BeliefState>(_fsm);
          for(auto state : _nodes)
            {
              std::for_each(_fsm.inputEventTransitionBegin(state),
                            _fsm.inputEventTransitionEnd(state),
                            [&](typename Fsm::Transition trans)
                            {
                                result->insertState(trans.source());
                              });
              }
            return result;
          }


          template<typename Predicate>
            SharedPtr
            nextBeliefState(Predicate predicate) const
          {
            SharedPtr result = std::make_shared<BeliefState>(_fsm);
            for(auto state : _nodes)
              {
                std::for_each(_fsm.outputTransitionBegin(state),
                              _fsm.outputTransitionEnd(state),
                              [&](typename Fsm::Transition trans)
                              {
                                if(predicate(trans.target()))
                                  {
                                    result->insertState(trans.target());
                                  }
                              });
              }
            return result;
          }

          template<typename Predicate>
            SharedPtr
            prevBeliefState(Predicate predicate) const
          {
            SharedPtr result = std::make_shared<BeliefState>(_fsm);
            for(auto state : _nodes)
              {
                std::for_each(_fsm.inputEventTransitionBegin(state),
                              _fsm.inputEventTransitionEnd(state),
                              [&](typename Fsm::Transition trans)
                              {
                                if(predicate(trans.source()))
                                  {
                                    result->insertState(trans.source());
                                  }
                              });
              }
            return result;
          }



          template<typename TransitionPredicate, typename TransitionInsertIterator>
            SharedPtr
            nextReachableBeliefState(TransitionPredicate isGoal,
                                     TransitionInsertIterator transInsertIterator,
                                     TransitionInsertIterator targetTransInsertIterator)
          {
            return nextReachableBeliefState(isGoal,
                                            [&](Transition t){ return false; },
                                            transInsertIterator,
                                            targetTransInsertIterator);
          }


          template<typename IsGoal, typename IsForbidden, typename TransitionInsertIterator>
            SharedPtr
            nextReachableBeliefState(IsGoal isGoal, IsForbidden isForbidden,
                                     TransitionInsertIterator transInsertIterator,
                                     TransitionInsertIterator targetTransInsertIterator)
          {
          
            using TransitionMap = Diades::Graph::ConstEdgeMap<char>;
            SharedPtr result = std::make_shared<BeliefState>(_fsm);
            TransitionMap visited(_fsm.behaviour());
            std::stack<Transition> visitingTransitions;
            list<Transition> resultTransitions;
            std::for_each(begin(),end(),
                          [&](State s)
                          {
                            std::for_each(_fsm.outputTransitionBegin(s),
                                          _fsm.outputTransitionEnd(s),
                                          [&](Transition t){
                                            if(!isForbidden(t))
                                              {
                                                visitingTransitions.push(t);
                                              }
                                          });
                          });
            while (!visitingTransitions.empty()) {
              auto current = visitingTransitions.top();
              visitingTransitions.pop();
              if (visited[current] == 0)
                {
                  visited[current] = 1;
                  if(!isGoal(current))
                    {
                      std::for_each(_fsm.outputTransitionBegin(current.target()),
                                    _fsm.outputTransitionEnd(current.target()),
                                    [&](Transition t){
                                      if(!isForbidden(t) && visited[t] ==0)
                                        {
                                          visitingTransitions.push(t);
                                        }
                                    });
                    }
                  else
                    {
                      resultTransitions.push_back(current);
                    }
                }
            }
            while(!resultTransitions.empty())
              {
                result->insertState(resultTransitions.front().target());
              
                std::stack<Transition> visitingTransitions2;
                visitingTransitions2.push(resultTransitions.front());
                while (!visitingTransitions2.empty()) {
                  auto current = visitingTransitions2.top();
                  visitingTransitions2.pop();
                  if(visited[current]!=2)
                    {
                      if(isGoal(current))
                        {
                          *targetTransInsertIterator++=current;
                        }
                      else
                        {
                          *transInsertIterator++=current;
                        }
                      visited[current] = 2;
                      std::for_each(_fsm.inputTransitionBegin(current.source()),
                                    _fsm.inputTransitionEnd(current.source()),
                                    [&](Transition t){
                                      if(visited[t] ==1)
                                        {
                                          visitingTransitions2.push(t);
                                        }
                                    });
                    }
                }
                
                resultTransitions.pop_front();
              }
            return result;
          }
      };



      template<typename Fsm>
      using BsPtr = typename BeliefState<Fsm>::SharedPtr;


      template<typename Fsm>
      using CstBsMap = CstStMap<BsPtr<Fsm>>;


      template<typename Fsm>
      using BsMap = StMap<BsPtr<Fsm>>;
      
    }
  }
}

#endif /* __DIADES__AUTOMATA__EXPERIMENTAL__BELIEFSTATE__HH */