IOState.hpp

#pragma once
 
#include "state_representation/State.hpp"
 
#include "state_representation/exceptions/IONotFoundException.hpp"
#include "state_representation/exceptions/IncompatibleSizeException.hpp"
#include "state_representation/exceptions/InvalidCastException.hpp"
 
namespace state_representation {
 
template<typename T>
class IOState : public State {
public:
  unsigned int get_size() const;
 
  const std::vector<std::string>& get_names() const;
 
  unsigned int get_io_index(const std::string& io_name) const;
 
  T get_value(const std::string& name) const;
 
  T get_value(unsigned int io_index) const;
 
  Eigen::Vector<T, Eigen::Dynamic> data() const;
 
  Eigen::Array<T, Eigen::Dynamic, 1> array() const;
 
  void set_names(unsigned int nb_ios);
 
  void set_names(const std::vector<std::string>& names);
 
  void set_value(T value, const std::string& name);
 
  void set_value(T value, unsigned int io_index);
 
  void set_data(const Eigen::Vector<T, Eigen::Dynamic>& data);
 
  void set_data(const std::vector<T>& data);
 
  bool is_incompatible(const State& state) const override;
 
  std::vector<T> to_std_vector() const;
 
protected:
  IOState() = default;
 
  explicit IOState(const std::string& name, unsigned int nb_ios);
 
  IOState(const std::string& name, const std::vector<std::string>& io_names);
 
  friend void swap(IOState& state1, IOState& state2) {
    swap(static_cast<State&>(state1), static_cast<State&>(state2));
    std::swap(state1.names_, state2.names_);
    std::swap(state1.data_, state2.data_);
  }
 
  static void assert_index_in_range(unsigned int io_index, unsigned int size);
 
  std::vector<std::string> names_;
  Eigen::Vector<T, Eigen::Dynamic> data_;
};
 
template<typename T>
IOState<T>::IOState(const std::string& name, unsigned int nb_ios) :
    State(name),
    names_(nb_ios) {
  this->set_names(nb_ios);
}
 
template<typename T>
IOState<T>::IOState(const std::string& name, const std::vector<std::string>& io_names) :
  IOState<T>(name, io_names.size()) {
  this->set_names(io_names);
}
 
template<typename T>
unsigned int IOState<T>::get_size() const {
  return this->names_.size();
}
 
template<typename T>
const std::vector<std::string>& IOState<T>::get_names() const {
  return this->names_;
}
 
template<typename T>
unsigned int IOState<T>::get_io_index(const std::string& io_name) const {
  auto finder = std::find(this->names_.begin(), this->names_.end(), io_name);
  if (finder == this->names_.end()) {
    throw exceptions::IONotFoundException("The IO with name '" + io_name + "' could not be found in the IO state.");
  }
  return std::distance(this->names_.begin(), finder);
}
 
template<typename T>
T IOState<T>::get_value(const std::string& io_name) const {
  return this->get_value(this->get_io_index(io_name));
}
 
template<typename T>
T IOState<T>::get_value(unsigned int io_index) const {
  this->assert_not_empty();
  IOState<T>::assert_index_in_range(io_index, this->get_size());
  return this->data_(io_index);
}
 
template<typename T>
Eigen::Vector<T, Eigen::Dynamic> IOState<T>::data() const {
  this->assert_not_empty();
  return this->data_;
}
 
template<typename T>
Eigen::Array<T, Eigen::Dynamic, 1> IOState<T>::array() const {
  this->assert_not_empty();
  return this->data_.array();
}
template<typename T>
void IOState<T>::set_names(unsigned int nb_ios) {
  if (this->get_size() != nb_ios) {
    throw exceptions::IncompatibleSizeException(
        "Input number of IOs is of incorrect size, expected " + std::to_string(this->get_size()) + " got "
            + std::to_string(nb_ios));
  }
  for (unsigned int i = 0; i < nb_ios; ++i) {
    this->names_[i] = "io" + std::to_string(i);
  }
  this->reset_timestamp();
}
 
template<typename T>
void IOState<T>::set_names(const std::vector<std::string>& names) {
  if (this->get_size() != names.size()) {
    throw exceptions::IncompatibleSizeException(
        "Input number of IOs is of incorrect size, expected " + std::to_string(this->get_size()) + " got "
            + std::to_string(names.size()));
  }
  this->names_ = names;
  this->reset_timestamp();
}
 
template<typename T>
void IOState<T>::set_value(T value, const std::string& io_name) {
  this->set_value(value, this->get_io_index(io_name));
}
 
template<typename T>
void IOState<T>::set_value(T value, unsigned int io_index) {
  IOState<T>::assert_index_in_range(io_index, this->get_size());
  this->data_(io_index) = value;
  this->set_empty(false);
}
 
template<typename T>
void IOState<T>::set_data(const Eigen::Vector<T, Eigen::Dynamic>& data) {
  if (data.size() != this->get_size()) {
    throw exceptions::IncompatibleSizeException(
        "Input is of incorrect size, expected " + std::to_string(this->get_size()) + ", got "
            + std::to_string(data.size()));
  }
  this->data_ = data;
  this->set_empty(false);
}
 
template<typename T>
bool IOState<T>::is_incompatible(const State& state) const {
  try {
    auto other = dynamic_cast<const IOState<T>&>(state);
    if (this->names_.size() != other.names_.size()) {
      return true;
    }
    for (unsigned int i = 0; i < this->names_.size(); ++i) {
      if (this->names_[i] != other.names_[i]) {
        return true;
      }
    }
    return false;
  } catch (const std::bad_cast& ex) {
    throw exceptions::InvalidCastException(std::string("Could not cast the given object to an IOState: ") + ex.what());
  }
}
 
template<typename T>
void IOState<T>::assert_index_in_range(unsigned int io_index, unsigned int size) {
  if (io_index > size) {
    throw exceptions::IONotFoundException(
        "Index '" + std::to_string(io_index) + "' is out of range for IO state with size" + std::to_string(size));
  }
}
 
}// namespace state_representation