sm_state.h

/*
 * Developer : Alexey Zakharov (alexey.zakharov@vectioneer.com)
 * All rights reserved. Copyright (c) 2014-2016 VECTIONEER.
 */
 
#ifndef STATE_MACHINE_SM_STATE_H
#define STATE_MACHINE_SM_STATE_H
 
#include "sg_monitorbase.h"
#include "sg_signalidgroup.h"
#include "sm_errorhandlebase.h"
#include "sm_event.h"
#include <algorithm>
#include <cstdint>
#include <limits>
#include <map>
#include <set>
#include <string>
#include <vector>
 
#define LOGIC_STATE_ID(x) x::id(mcx::state_machine::id_caller<x>())
 
namespace mcx::state_machine {
 
constexpr auto EVENT_DEFAULT_TIMEOUT_SEC = std::numeric_limits<double>::max();
 
template <class...>
struct id_caller {};
 
template <class SUPER_STATE>
class StateMachine;
 
template <class SUPER_STATE>
class State {
 
  template <class STATE>
  struct Container {
    static int id;
  };
 
  int* idPtr;
 
  StateMachine<SUPER_STATE>* mach;
  std::string defaultName;
 
  struct event_name {
    Event<SUPER_STATE>* event;
    std::string name;
  };
  std::vector<event_name> eventList;
 
  inline std::string stateName(const std::string& prettyFunction) {
    const std::string stateBeginToken = "STATE = ";
    const std::string stateEndTokenGCC = ";";
    const std::string stateEndTokenClang = ",";
    size_t begin = prettyFunction.find(stateBeginToken) + stateBeginToken.length();
    size_t end = prettyFunction.find(stateEndTokenGCC, begin);
    if (end == std::string::npos) {
      end = prettyFunction.find(stateEndTokenClang, begin);
    }
    if (begin == std::string::npos || end == std::string::npos)
      return "Undefined";
 
    return prettyFunction.substr(begin, end - begin);
  }
 
  struct ErrorMonitorData {
    ErrorHandleBase* errorHandle{};
    ErrorGroup errors[signal_monitor::MonitorAction::NUMBER_OF_ACTIONS];
  };
 
  ErrorMonitorData errorMonitor;
 
protected:
  void addToEventList(const event_name& event) {
    for (auto& e : eventList) {
      if (event.event->compare(e.event)) {
        delete event.event; // event already exits, need to remove
        return;
      }
    }
    eventList.push_back(event);
  }
 
  void addEventName(const Event0<SUPER_STATE>& event, const std::string& eventName) {
    addToEventList({.event = event.getEvent(), .name = eventName});
  }
 
  void addEventName(const Event1<SUPER_STATE>& event, const std::string& eventName) {
    eventList.push_back({.event = event.getEvent(), .name = eventName});
  }
 
  void addEventName(const Event2<SUPER_STATE>& event, const std::string& eventName) {
    eventList.push_back({.event = event.getEvent(), .name = eventName});
  }
 
  void addEventName(const Event3<SUPER_STATE>& event, const std::string& eventName) {
    eventList.push_back({.event = event.getEvent(), .name = eventName});
  }
 
public:
  template <class STATE, class MACHINE>
  void create(MACHINE* newMachine, const STATE& state, int id, ErrorHandleBase* errorHandle) {
    setMachine(newMachine);
    Container<STATE>::id = id;
    idPtr = &Container<STATE>::id;
    errorMonitor.errorHandle = errorHandle;
    defaultName = stateName(__PRETTY_FUNCTION__);
    this->addEventName(&SUPER_STATE::delayEvent, "delayEvent");
    this->addEventName(&SUPER_STATE::terminateEvent, "terminateEvent");
    this->addEventName(&SUPER_STATE::acknowledgeErrors, "acknowledgeErrors");
    this->addEventName(&SUPER_STATE::warning_, "warning");
    this->addEventName(&SUPER_STATE::forcedDisengaged_, "forcedDisengaged");
    this->addEventName(&SUPER_STATE::shutdown_, "shutdown");
    this->addEventName(&SUPER_STATE::emergencyStop_, "emergencyStop");
    registerUserEvents();
  }
 
  void addErrorsHelper(ErrorGroup* stateErrors, const ErrorGroup& errorsToAdd) {
    auto groupBegin = stateErrors->begin();
    for (auto& error : errorsToAdd) {
      groupBegin = std::find(groupBegin, stateErrors->end(), error);
      if (groupBegin == stateErrors->end()) {
        stateErrors->add(error);
      }
      ++groupBegin;
    }
  }
 
  void enableErrorsHelper(ErrorGroup* stateErrors, const ErrorGroup& refErrors, const ErrorGroup& errorsToAdd) {
    for (auto& error : errorsToAdd) {
      auto refBegin = refErrors.begin();
      while (true) {
        // check every new error, if it exists in the reference group
        refBegin = std::find(refBegin, refErrors.end(), error);
        if (refBegin != refErrors.end()) {
          auto groupBegin = std::find(stateErrors->begin(), stateErrors->end(), *refBegin);
          if (groupBegin == stateErrors->end()) {
            stateErrors->add(*refBegin);
          }
        } else {
          break;
        }
        ++refBegin;
      }
    }
  }
 
  void disableErrorsHelper(ErrorGroup* stateErrors, const ErrorGroup& errorsToAdd) {
    auto groupBegin = stateErrors->begin();
    for (auto& error : errorsToAdd) {
      while (true) {
        groupBegin = std::find(groupBegin, stateErrors->end(), error);
        if (groupBegin != stateErrors->end()) {
          stateErrors->erase(groupBegin);
        } else {
          break;
        }
      }
    }
  }
 
  void addErrors(const ErrorGroup& codes, signal_monitor::MonitorAction newAction) {
    auto& action = errorMonitor.errors;
    using namespace signal_monitor;
    auto& stateErrorsGroup = action[MonitorAction::ADD];
    switch (newAction) {
    case MonitorAction::ADD: {
      addErrorsHelper(&stateErrorsGroup, codes);
      break;
    }
    case MonitorAction::RESET: {
      auto& resetGroup = action[MonitorAction::RESET];
      enableErrorsHelper(&resetGroup, stateErrorsGroup, codes);
      break;
    }
    case MonitorAction::ENABLE: {
      auto& enableGroup = action[MonitorAction::ENABLE];
      enableErrorsHelper(&enableGroup, stateErrorsGroup, codes);
      auto& disableGroup = action[MonitorAction::DISABLE];
      disableErrorsHelper(&disableGroup, codes);
      break;
    }
    case MonitorAction::DISABLE: {
      auto& disableGroup = action[MonitorAction::DISABLE];
      enableErrorsHelper(&disableGroup, stateErrorsGroup, codes);
      auto& enableGroup = action[MonitorAction::ENABLE];
      disableErrorsHelper(&enableGroup, codes);
      break;
    }
    default:
      log_error("Failed to add unknown motion action: {}", newAction);
      break;
    }
  }
 
  virtual void registerUserEvents() {}
 
  template <class MACHINE>
  void setMachine(MACHINE* newMachine) {
    mach = newMachine;
  }
 
  template <class MACHINE>
  MACHINE* getMachine() {
    return mach;
  }
 
  State() : mach() { idPtr = nullptr; }
 
  virtual ~State() {
    for (auto& e : eventList) {
      delete e.event;
    }
    eventList.clear();
  }
 
  double getDtSec() const { return mach->getDtSec(); }
 
  template <class STATE>
  static int id(id_caller<STATE>) {
    return Container<STATE>::id;
  }
 
  int id() { return *idPtr; }
 
  void clearEventQueue(bool highPriority = true, bool lowPriority = true) { mach->clear(highPriority, lowPriority); }
 
  SUPER_STATE* setActiveState(int state_id) { return mach->setActiveState(state_id, true); }
 
  template <class NEWSTATE>
  NEWSTATE* setActiveState() {
    return mach->template setActiveState<NEWSTATE>(true);
  }
 
  template <class NEWSTATE>
  NEWSTATE* setActiveState(const NEWSTATE& state) {
    return mach->template setActiveState<NEWSTATE>(true);
  }
 
  template <typename... STATES>
  bool isStateActive() {
    return mach->template isStateActive<STATES...>();
  }
 
  bool isStateActive(int id) { return mach->isStateActive(id); }
 
  void addEvent(const Event0<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = HIGH_PRIORITY_EVENT) {
    mach->addEvent(event, timeoutSec, priority);
  }
 
  void addEvent(const Event1<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = HIGH_PRIORITY_EVENT) {
    mach->addEvent(event, timeoutSec, priority);
  }
 
  void addEvent(const Event2<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = HIGH_PRIORITY_EVENT) {
    mach->addEvent(event, timeoutSec, priority);
  }
 
  void addEvent(const Event3<SUPER_STATE>& event, double timeoutSec = EVENT_DEFAULT_TIMEOUT_SEC,
                EventPriority priority = HIGH_PRIORITY_EVENT) {
    mach->addEvent(event, timeoutSec, priority);
  }
 
  double getActiveTimeoutSec() { return mach->getActiveTimeoutSec(); }
 
  virtual const char* stateName() const { return defaultName.c_str(); }
 
  virtual const char* eventName(Event<SUPER_STATE>* event) {
    for (unsigned int i = 0; i < eventList.size(); i++) {
      if (event->compare(eventList[i].event)) {
        return eventList[i].name.c_str();
      }
    }
 
    return "Undefined name";
  }
 
  virtual EventStatus acknowledgeErrors() { return EVENT_NONE; }
 
  bool clearErrorMonitor(bool warnings_only = false) { return getErrorMonitor()->acknowledge(warnings_only); }
 
  ErrorHandleBase* getErrorMonitor() { return errorMonitor.errorHandle; }
 
  EventStatus warning(const Error& error) {
    auto result = mach->executeEvent({&SUPER_STATE::warning_, error}, true);
    if (result == EventStatus::EVENT_DONE) {
      if (getErrorMonitor()->warning(error)) {
        return EVENT_DONE;
      }
    }
    return EVENT_NONE;
  }
 
  EventStatus forcedDisengaged(const Error& error) {
    auto result = mach->executeEvent({&SUPER_STATE::forcedDisengaged_, error});
    if (result == EventStatus::EVENT_DONE) {
      if (getErrorMonitor()->forcedDisengaged(error)) {
        return EVENT_DONE;
      }
    }
    return EVENT_NONE;
  }
 
  EventStatus shutdown(const Error& error) {
    auto result = mach->executeEvent({&SUPER_STATE::shutdown_, error});
    if (result == EventStatus::EVENT_DONE) {
      if (getErrorMonitor()->shutdown(error)) {
        return EVENT_DONE;
      }
    }
    return EVENT_NONE;
  }
 
  EventStatus emergencyStop(const Error& error) {
    auto result = mach->executeEvent({&SUPER_STATE::emergencyStop_, error});
    if (result == EventStatus::EVENT_DONE) {
      if (getErrorMonitor()->emergencyStop(error)) {
        return EVENT_DONE;
      }
    }
    return EVENT_NONE;
  }
 
  EventStatus delayEvent() { return EVENT_REPEAT_NO_TERMINATE; }
 
  virtual EventStatus terminateEvent() { return EVENT_DONE; }
 
  void errorMonitorAction() {
    auto monitor = errorMonitor.errorHandle->getSignalMonitor();
    using namespace signal_monitor;
    for (int action = 0; action < MonitorAction::NUMBER_OF_ACTIONS; action++) {
      auto& errors = errorMonitor.errors[action];
      switch (action) {
      case MonitorAction::ENABLE:
        monitor->enable(errors);
        break;
      case MonitorAction::DISABLE:
        monitor->disable(errors);
        break;
      case MonitorAction::RESET:
        monitor->reset(errors);
        break;
      default:
        break;
      }
    }
  }
 
  virtual void enter() {}
 
  virtual void enter(int prev_state_id) {}
 
  virtual void leave() {}
 
  virtual void leave(int next_state_id) {}
 
  virtual void iterate(double dt_sec) {}
 
protected:
  virtual EventStatus warning_(const Error& error) = 0;
 
  virtual EventStatus forcedDisengaged_(const Error& error) = 0;
 
  virtual EventStatus shutdown_(const Error& error) = 0;
 
  virtual EventStatus emergencyStop_(const Error& error) = 0;
};
 
} // namespace mcx::state_machine
 
#endif // STATE_MACHINE_SM_STATE_H