sm_statemachine.h

/*
 * Developer : Alexey Zakharov (alexey.zakharov@vectioneer.com)
 * All rights reserved. Copyright (c) 2014-2018 VECTIONEER.
 */
 
#ifndef STATE_MACHINE_SM_STATEMACHINE_H_
#define STATE_MACHINE_SM_STATEMACHINE_H_
 
#include "sg_monitor.h"
#include "sm_errorhandle.h"
#include "sm_event.h"
#include "sm_state.h"
#include "utl_log.h"
#include <deque>
#include <initializer_list>
#include <string>
 
namespace mcx::state_machine {
 
constexpr auto MAX_NUMBER_OF_STATES = 100;
constexpr auto DEFAULT_TERMINATE_EVENT = nullptr;
 
template <class SUPER_STATE>
class StateMachine {
  friend class ErrorHandle<SUPER_STATE>;
 
  SUPER_STATE* activeState;
  int activeStateId;
  SUPER_STATE* stateArray[MAX_NUMBER_OF_STATES];
  std::string machineName;
  unsigned int eventStackOffset{0};
  double dtSec{0};
 
  bool setActiveStateRoutine(int id) {
    log_assert(id > -1, "State is not initialized!");
    if (id > -1) {
      SUPER_STATE* newState = stateArray[id];
      if (newState) {
        activeState = newState;
        activeStateId = id;
        return true;
      }
    }
    return false;
  }
 
  std::deque<Event<SUPER_STATE>*> highPriorityEvents;
  std::deque<Event<SUPER_STATE>*> lowPriorityEvents;
 
  int numberOfStates;
 
  EventStatus executeEvent(Event<SUPER_STATE>* event, bool silently) {
    EventStatus status = EVENT_NONE;
    if (activeState) {
      auto prevState = activeState;
      status = event->dispatchEvent(activeState);
      if ((!event->getExecutedOnceFlag() || (status != EVENT_REPEAT && status != EVENT_REPEAT_NO_TERMINATE)) &&
          !silently && status != EVENT_NONE) {
        log_sm("{}: State: {} Event: {} {}", getName(), prevState->stateName(), prevState->eventName(event),
               status == EVENT_REPEAT || status == EVENT_REPEAT_NO_TERMINATE ? "[repeat]"
               : status == EVENT_DONE                                        ? "[done]"
               : status == EVENT_TERMINATE                                   ? "[terminate]"
                                                                             : "[unknown]");
      }
    } else {
      log_sm("{}: Active state is not set, can not execute event!", getName());
    }
 
    return status;
  }
 
  SUPER_STATE* getStatePtr(uint32_t index) {
    if (static_cast<int>(index) < numberOfStates) {
      return stateArray[index];
    }
    return nullptr;
  }
 
  template <typename... STATES>
  void action_variadic(signal_monitor::SignalMonitorBase* monitor, const ErrorGroup& error_group,
                       signal_monitor::MonitorAction action, id_caller<STATES>... states) {
    auto id_list = {(STATES::id(id_caller<STATES>()))...};
    for (auto& id : id_list) {
      stateArray[id]->addErrors(monitor, error_group, action);
    }
  }
 
  // dummy for empty parameter pack
 
  void action_variadic(signal_monitor::SignalMonitorBase* monitor, const ErrorGroup& error_group,
                       signal_monitor::MonitorAction action) {}
 
  state_machine::ErrorHandle<SUPER_STATE> errorHandle;
 
  void addEvent(Event<SUPER_STATE>* event, double timeoutSec, EventPriority priority,
                EventStatus(SUPER_STATE::*terminateEvent)) {
    event->setTimeoutSec(timeoutSec);
    if (!terminateEvent) {
      event->setTerminateFunc(&State<SUPER_STATE>::terminateEvent);
    }
    if (priority == HIGH_PRIORITY_EVENT) {
      highPriorityEvents.insert(highPriorityEvents.begin() + eventStackOffset, event);
      ++eventStackOffset;
      //      highPriorityEvents.push_back(event);
    } else {
      lowPriorityEvents.push_back(event);
    }
  }
 
public:
  StateMachine() : stateArray(), errorHandle(this) {
    activeState = nullptr;
    numberOfStates = 0;
    activeStateId = -1;
    eventStackOffset = 0;
  }
 
  virtual ~StateMachine() { deinitialize(); }
 
  void deinitialize() {
    for (auto& i : stateArray) {
      if (i) {
        delete i;
        i = nullptr;
      }
    }
 
    for (auto& e : highPriorityEvents) {
      delete e;
    }
 
    for (auto& e : lowPriorityEvents) {
      delete e;
    }
 
    highPriorityEvents.clear();
    lowPriorityEvents.clear();
  }
 
  void setName(const std::string& new_name) {
    machineName = new_name;
    errorHandle.setName(new_name);
  }
 
  const std::string& getName() const { return machineName; }
 
  template <class STATE>
  void createState() {
    int id = STATE::id(id_caller<STATE>());
    if (id == -1) {
      id = numberOfStates++;
    } else {
      numberOfStates = numberOfStates < (id + 1) ? id + 1 : numberOfStates;
    }
 
    if (!stateArray[id]) {
      stateArray[id] = new STATE();
    }
    stateArray[id]->create(this, STATE(), id, &errorHandle);
    log_sm("{}: Initialized state: {}", getName(), stateArray[id]->stateName());
  }
 
  template <class STATE>
  void createState(const STATE& new_state) {
    int id = STATE::id(id_caller<STATE>());
    if (id == -1) {
      id = numberOfStates++;
    } else {
      numberOfStates = numberOfStates < (id + 1) ? id + 1 : numberOfStates;
    }
 
    log_assert(id <= MAX_NUMBER_OF_STATES, "Number of states exceeded maximum value: {}!", MAX_NUMBER_OF_STATES);
 
    if (!stateArray[id]) {
      stateArray[id] = new STATE(new_state);
    }
 
    stateArray[id]->create(this, new_state, id, &errorHandle);
    log_sm("{}: Initialized state: {}", getName(), stateArray[id]->stateName());
  }
 
  template <class STATE>
  STATE* setActiveState(bool output = true) {
    int id = STATE::id(id_caller<STATE>());
    return dynamic_cast<STATE*>(setActiveState(id, output));
  }
 
  SUPER_STATE* setActiveState(int state_id, bool output = true) {
    auto prevStateId = activeStateId;
    if (activeState) {
      activeState->leave();
      activeState->leave(state_id);
    }
    if (setActiveStateRoutine(state_id)) {
      if (output) {
        log_sm("{}: Enter state: {}", getName(), activeState->stateName());
      }
      activeState->errorMonitorAction();
      activeState->enter();
      activeState->enter(prevStateId);
      return activeState;
    } else {
      log_sm("{}: Can not enter state, it is not initialized!", getName());
    }
 
    return NULL;
  }
 
  template <typename STATE>
  int getStateId() {
    return STATE::id(id_caller<STATE>());
  }
 
  template <typename... STATES>
  bool isStateActive() {
    if (activeStateId > -1) {
      auto id_list = {(STATES::id(id_caller<STATES>()))...};
      for (auto& id : id_list) {
        if (id == activeStateId) {
          return true;
        }
      }
      //      return activeStateId == STATE::id(id_caller<STATE>()) ? true : false;
    }
    return false;
  }
 
  bool isStateActive(int id) {
    if (activeStateId > -1) {
      return activeStateId == id;
    }
    return false;
  }
 
  bool isStateActive(std::initializer_list<int> id_list) {
    if (activeStateId > -1) {
      for (auto& id : id_list) {
        if (id == activeStateId) {
          return true;
        }
      }
    }
    return false;
  }
 
  EventStatus executeEvent(const Event0<SUPER_STATE>& event, bool silently = false) {
    EventStatus status = executeEvent(event.getEvent(), silently);
    delete event.getEvent();
    return status;
  }
 
  EventStatus executeEvent(const Event1<SUPER_STATE>& event, bool silently = false) {
    EventStatus status = executeEvent(event.getEvent(), silently);
    delete event.getEvent();
    return status;
  }
 
  EventStatus executeEvent(const Event2<SUPER_STATE>& event, bool silently = false) {
    EventStatus status = executeEvent(event.getEvent(), silently);
    delete event.getEvent();
    return status;
  }
 
  EventStatus executeEvent(const Event3<SUPER_STATE>& event, bool silently = false) {
    EventStatus status = executeEvent(event.getEvent(), silently);
    delete event.getEvent();
    return status;
  }
 
  void addEvent(const Event0<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = LOW_PRIORITY_EVENT,
                EventStatus(SUPER_STATE::*terminateEvent) = DEFAULT_TERMINATE_EVENT) {
    addEvent(event.getEvent(), timeoutSec, priority, terminateEvent);
  }
 
  void addEvent(const Event1<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = LOW_PRIORITY_EVENT,
                EventStatus(SUPER_STATE::*terminateEvent) = DEFAULT_TERMINATE_EVENT) {
    addEvent(event.getEvent(), timeoutSec, priority, terminateEvent);
  }
 
  void addEvent(const Event2<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = LOW_PRIORITY_EVENT,
                EventStatus(SUPER_STATE::*terminateEvent) = DEFAULT_TERMINATE_EVENT) {
    addEvent(event.getEvent(), timeoutSec, priority, terminateEvent);
  }
 
  void addEvent(const Event3<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = LOW_PRIORITY_EVENT,
                EventStatus(SUPER_STATE::*terminateEvent) = DEFAULT_TERMINATE_EVENT) {
    addEvent(event.getEvent(), timeoutSec, priority, terminateEvent);
  }
 
  double getDtSec() const { return dtSec; }
 
  EventStatus iterate(double cycleTimeSec) {
    dtSec = cycleTimeSec;
 
    EventStatus status = EVENT_NONE;
 
    errorHandle.iterate(cycleTimeSec);
 
    if (activeState) {
      activeState->iterate(cycleTimeSec);
    }
 
    if (highPriorityEvents.empty() && !lowPriorityEvents.empty()) {
      highPriorityEvents.push_back(lowPriorityEvents.front());
      lowPriorityEvents.pop_front();
    }
 
    if (!highPriorityEvents.empty()) {
      Event<SUPER_STATE>* event = highPriorityEvents.front();
      eventStackOffset = 0;
      status = executeEvent(event, false);
      event->setExecutedOnceFlag(true);
      if (status == EVENT_REPEAT || status == EVENT_REPEAT_NO_TERMINATE) {
        double timeout = event->getTimeoutSec() - cycleTimeSec;
        event->setTimeoutSec(timeout);
        if (timeout <= 0.0f) {
          highPriorityEvents.erase(highPriorityEvents.begin() + eventStackOffset);
          //          highPriorityEvents.pop_front();
          switch (status) {
          case EVENT_REPEAT: {
            status = EVENT_TERMINATE;
            Event<SUPER_STATE>* termFuncPtr = Event0<SUPER_STATE>(event->getTerminateFunc()).getEvent();
            if (termFuncPtr) {
              highPriorityEvents.push_front(termFuncPtr);
            }
            break;
          }
          case EVENT_REPEAT_NO_TERMINATE: {
            status = EVENT_DONE;
            break;
          }
          default:
            break;
          }
          delete event;
        }
      } else {
        highPriorityEvents.erase(highPriorityEvents.begin() + eventStackOffset);
        //        highPriorityEvents.pop_front();
        delete event;
      }
    }
 
    return status;
  }
 
  void clear(bool highPriority = true, bool lowPriority = true) {
    if (highPriority) {
      eventStackOffset = 0;
      highPriorityEvents.clear();
    }
    if (lowPriority) {
      lowPriorityEvents.clear();
    }
  }
 
  size_t length(EventPriority priority = LOW_PRIORITY_EVENT) {
    switch (priority) {
    case LOW_PRIORITY_EVENT: {
      return lowPriorityEvents.size();
    }
    case HIGH_PRIORITY_EVENT: {
      return highPriorityEvents.size();
    }
    }
 
    return 0;
  }
 
  ErrorHandle<SUPER_STATE>& getErrorHandle() { return errorHandle; }
 
  const ErrorHandle<SUPER_STATE>& getErrorHandle() const { return errorHandle; }
 
  bool warning(const Error& error) {
    if (activeState) {
      return activeState->warning(error) == EventStatus::EVENT_DONE;
    }
    return false;
  }
 
  bool forcedDisengaged(const Error& error) {
    if (activeState) {
      return activeState->forcedDisengaged(error) == EventStatus::EVENT_DONE;
    }
    return false;
  }
 
  bool shutdown(const Error& error) {
    if (activeState) {
      return activeState->shutdown(error) == EventStatus::EVENT_DONE;
    }
    return false;
  }
 
  bool emergencyStop(const Error& error) {
    if (activeState) {
      return activeState->emergencyStop(error) == EventStatus::EVENT_DONE;
    }
    return false;
  }
};
 
template <class BaseState>
template <class STATE>
int State<BaseState>::Container<STATE>::id = -1;
 
} // namespace mcx::state_machine
 
#endif // STATE_MACHINE_SM_STATEMACHINE_H_