state_manager.h

// NIST-developed software is provided by NIST as a public service. You may use, copy and distribute copies of the
// software in any medium, provided that you keep intact this entire notice. You may improve, modify and create
// derivative works of the software or any portion of the software, and you may copy and distribute such modifications
// or works. Modified works should carry a notice stating that you changed the software and should note the date and
// nature of any such change. Please explicitly acknowledge the National Institute of Standards and Technology as the
// source of the software. NIST-developed software is expressly provided "AS IS." NIST MAKES NO WARRANTY OF ANY KIND,
// EXPRESS, IMPLIED, IN FACT OR ARISING BY OPERATION OF LAW, INCLUDING, WITHOUT LIMITATION, THE IMPLIED WARRANTY OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, NON-INFRINGEMENT AND DATA ACCURACY. NIST NEITHER REPRESENTS NOR
// WARRANTS THAT THE OPERATION OF THE SOFTWARE WILL BE UNINTERRUPTED OR ERROR-FREE, OR THAT ANY DEFECTS WILL BE
// CORRECTED. NIST DOES NOT WARRANT OR MAKE ANY REPRESENTATIONS REGARDING THE USE OF THE SOFTWARE OR THE RESULTS
// THEREOF, INCLUDING BUT NOT LIMITED TO THE CORRECTNESS, ACCURACY, RELIABILITY, OR USEFULNESS OF THE SOFTWARE. You
// are solely responsible for determining the appropriateness of using and distributing the software and you assume
// all risks associated with its use, including but not limited to the risks and costs of program errors, compliance
// with applicable laws, damage to or loss of data, programs or equipment, and the unavailability or interruption of 
// operation. This software is not intended to be used in any situation where a failure could cause risk of injury or
// damage to property. The software developed by NIST employees is not subject to copyright protection within the
// United States.


#ifndef HEDGEHOG_STATE_MANAGER_H
#define HEDGEHOG_STATE_MANAGER_H

#include "../abstract_task.h"
#include "abstract_state.h"

namespace hh {
// Have to add -Woverloaded-virtual for clang because execute hides overloaded virtual function
#if defined (__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Woverloaded-virtual"
#endif //__clang//

namespace behavior {

template<class StateOutput, class ...StateInputs>
class BaseStateManager : public AbstractTask<StateOutput, StateInputs...> {
 public:
  BaseStateManager()
      : AbstractTask<StateOutput, StateInputs...>("StateManager", 1, core::NodeType::StateManager, false) {}

};

template<class StateInput, class StateOutput, class ...StateInputs>
class StateManagerExecuteDefinition : public virtual BaseStateManager<StateOutput, StateInputs...> {
 private:
  std::shared_ptr<AbstractState<StateOutput, StateInputs...>> es_; 
 public:
  StateManagerExecuteDefinition() = delete;
  explicit StateManagerExecuteDefinition(std::shared_ptr<AbstractState<StateOutput, StateInputs...>> const &state)
      : es_(state) { }

  virtual ~StateManagerExecuteDefinition() = default;

  void execute(std::shared_ptr<StateInput> input) final {
    this->es_->lock();
    std::static_pointer_cast<behavior::Execute<StateInput>>(this->es_)->execute(input);
    while (!this->es_->readyList()->empty()) { this->addResult(this->es_->frontAndPop()); }
    this->es_->unlock();
  }
};

}
#if defined (__clang__)
#pragma clang diagnostic pop
#endif //__clang//

template<class StateOutput, class ...StateInputs>
 class StateManager : public behavior::StateManagerExecuteDefinition<StateInputs, StateOutput, StateInputs...> ... {

 private:
  std::shared_ptr<AbstractState<StateOutput, StateInputs...>> state_ = nullptr; 

 public:
  using behavior::StateManagerExecuteDefinition<StateInputs, StateOutput, StateInputs...>::execute...;

  StateManager() = delete;

  template<
      class StateType,
      class IsCompatibleState = typename std::enable_if_t<
          std::is_base_of_v<AbstractState<StateOutput, StateInputs...>, StateType>
      >
  >
  explicit StateManager(std::shared_ptr<StateType> const state) :
      behavior::StateManagerExecuteDefinition<StateInputs, StateOutput, StateInputs...>(state)...,
  state_(state) {}

  template<
      class StateType,
      class IsCompatibleState = typename std::enable_if_t<
          std::is_base_of_v<AbstractState<StateOutput, StateInputs...>, StateType>
      >
  >
  StateManager(std::shared_ptr<StateType> const state, bool automaticStart) :
      behavior::StateManagerExecuteDefinition<StateInputs, StateOutput, StateInputs...>(state)...,
  state_(state) {
    std::dynamic_pointer_cast<core::CoreTask<StateOutput, StateInputs...>>(this->core())->automaticStart(automaticStart);
  }

  template<
      class StateType,
      class IsCompatibleState = typename std::enable_if_t<
          std::is_base_of_v<AbstractState<StateOutput, StateInputs...>, StateType>
      >
  >
  StateManager(std::string_view const name,
               std::shared_ptr<StateType> const state,
               bool automaticStart = false) :
      behavior::StateManagerExecuteDefinition<StateInputs, StateOutput, StateInputs...>(state)..., state_(state) {
    this->core()->name(name);
    std::dynamic_pointer_cast<core::CoreTask<StateOutput, StateInputs...>>(this->core())->automaticStart(automaticStart);
  }

  std::shared_ptr<AbstractTask<StateOutput, StateInputs...>> copy() override {
    return std::make_shared<StateManager<StateOutput, StateInputs...>>(this->name(),
                                                                       this->state(),
                                                                       this->automaticStart());
  }

 protected:
  std::shared_ptr<AbstractState<StateOutput, StateInputs...>> const &state() const { return state_; }
};
}
#endif //HEDGEHOG_STATE_MANAGER_H