StateMachine.h

Go to the documentation of this file.

// -*- C++ -*-
#ifndef RTC_STATEMACHINE_H
#define RTC_STATEMACHINE_H

#include <cassert>
#include <vector>
#include <coil/Mutex.h>
#include <coil/Guard.h>

namespace RTC_Utils
{
  template <class State>
  struct StateHolder
  {
    State curr;
    State prev;
    State next;
  };
  
  template <class State,
        class Listener,
        class States = StateHolder<State>, 
        class Callback = void (Listener::*)(const States& states)
        >
  class StateMachine
  {
    typedef coil::Mutex Mutex;
    typedef coil::Guard<Mutex> Guard;
  public:
    StateMachine(int num_of_state)
      : m_num(num_of_state),
        m_listener(NULL),
        m_entry(m_num, (Callback)NULL),
        m_predo(m_num, (Callback)NULL),
        m_do(m_num, (Callback)NULL),
        m_postdo(m_num, (Callback)NULL),
        m_exit(m_num, (Callback)NULL),
        m_transit(NULL)
    {
    };

    virtual ~StateMachine()
    {
    };

    StateMachine(const StateMachine& other)
      : m_num(other.m_num),
        m_listener(other.m_listener),
        m_entry (other.m_entry),
        m_predo (other.m_predo),
        m_do    (other.m_do),
        m_postdo(other.m_postdo),
        m_exit  (other.m_exit),
        m_transit(other.m_transit),
        m_states(other.m_states),
        m_selftrans(other.m_selftrans)
    {
    }

    StateMachine& operator=(const StateMachine& other)
    {
      StateMachine temp(other);
      swap(temp);
      return *this;
    }

    void swap(StateMachine& other)
    {
      std::swap(m_num,       other.m_num);
      std::swap(m_listener,  other.m_listener);
      std::swap(m_entry,     other.m_entry);
      std::swap(m_predo,     other.m_predo);
      std::swap(m_do,        other.m_do);
      std::swap(m_postdo,    other.m_postdo);
      std::swap(m_exit,      other.m_exit);
      std::swap(m_transit,   other.m_transit);
      std::swap(m_states,    other.m_states);
      std::swap(m_selftrans, other.m_selftrans);
    }
    void setNOP(Callback call_back)
    {
      setNullFunc(m_entry,  call_back);
      setNullFunc(m_do,     call_back);
      setNullFunc(m_exit,   call_back);
      setNullFunc(m_predo,  call_back);
      setNullFunc(m_postdo, call_back);
      m_transit = call_back;
    }
    
    void setListener(Listener* listener)
    {
      assert(listener != NULL);
      m_listener = listener;
    }
    
    bool setEntryAction(State state, Callback call_back)
    {
     try
        {
          m_entry.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
    
    bool setPreDoAction(State state, Callback call_back)
    {
      try
        {
          m_predo.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
    
    bool setDoAction(State state, Callback call_back)
    {
      try
        {
          m_do.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
    
    bool setPostDoAction(State state, Callback call_back)
    {
      try
        {
          m_postdo.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
    
    bool setExitAction(State state, Callback call_back)
    {
      try
        {
          m_exit.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
    
    bool setTransitionAction(Callback call_back)
    {
      m_transit = call_back;
      return true;
    }
    
    void setStartState(States states)
    {
      m_states.curr = states.curr;
      m_states.prev = states.prev;
      m_states.next = states.next;
    }
    
    States getStates()
    {
      Guard guard(m_mutex);
      return m_states;
    }
    
    State getState()
    {
      Guard guard(m_mutex);
      return m_states.curr;
    }
    
    bool isIn(State state)
    {
      Guard guard(m_mutex);
      return m_states.curr == state ? true : false;
    }
    
    void goTo(State state)
    {
      Guard guard(m_mutex);
      m_states.next = state;
      if (m_states.curr == state)
        {
          m_selftrans  = true;
        }
    }

    
    void worker()
    {
      States state;

      sync(state);

      if (state.curr == state.next)
        {
          // pre-do
          if (m_predo[state.curr] != NULL)
            (m_listener->*m_predo [state.curr])(state);

          if (need_trans()) return;

          // do
          if (m_do[state.curr] != NULL)
            (m_listener->*m_do    [state.curr])(state);

          if (need_trans()) return;

          // post-do
          if (m_postdo[state.curr] != NULL)
            (m_listener->*m_postdo[state.curr])(state);
        }
      else
        {
          if (m_exit[state.curr] != NULL)
            (m_listener->*m_exit[state.curr])(state);

          sync(state);

          if (state.curr != state.next)
            {
              state.curr = state.next;
              if(m_entry[state.curr] != NULL)
                (m_listener->*m_entry[state.curr])(state);
              update_curr(state.curr);
            }
        }
    }

    //============================================================
    // divided worker functions
    // The following divided worker functions have to be used together.
    // - worker_pre()
    // - worker_do()
    // - worker_post()
    //
    void worker_pre()
    {
      States state;
      sync(state);
      if (state.curr == state.next)
        {
          if (m_predo[state.curr] != NULL)
            {
              (m_listener->*m_predo[state.curr])(state);
            }
          return;
        }

      // State changed
      if (m_exit[state.curr] != NULL)
        {
          (m_listener->*m_exit[state.curr])(state);
        }
      sync(state);
      if (state.curr != state.next)
        {
          state.curr = state.next;
          if(m_entry[state.curr] != NULL)
            {
              (m_listener->*m_entry[state.curr])(state);
            }
          update_curr(state.curr);
        }
    }

    void worker_do()
    {
      States state;
      sync(state);
      if (m_do[state.curr] != NULL)
        {
          (m_listener->*m_do[state.curr])(state);
        }
    }

    void worker_post()
    {
      States state;
      sync(state);
      if (m_postdo[state.curr] != NULL)
        {
          (m_listener->*m_postdo[state.curr])(state);
        }
    }

  protected:
    void setNullFunc(std::vector<Callback>& s, Callback nullfunc)
    {
      s.clear();
      //      assert((size_t)m_num == s.size());
      for (size_t i(0); i < m_num; ++i)
        {
          s.push_back(nullfunc);
        }
    }

    int m_num;

    Listener* m_listener;

    std::vector<Callback> m_entry;

    std::vector<Callback> m_predo;

    std::vector<Callback> m_do;

    std::vector<Callback> m_postdo;

    std::vector<Callback> m_exit;

    Callback  m_transit;

    States m_states;
    bool m_selftrans;
    Mutex m_mutex;

  private:
    inline void sync(States& st)
    {
      Guard guard(m_mutex);
      st = m_states;
    }

    inline bool need_trans()
    {
      Guard guard(m_mutex);
      return (m_states.curr != m_states.next);
    }

    inline void update_curr(const State curr)
    {
      Guard guard(m_mutex);
      m_states.curr = curr;
    }
  };
}; // namespace RTC_Utils

#endif // RTC_STATEMACHINE_H