sg_monitor.h

/*
 * Developer : Alexey Zakharov (alexey.zakharov@vectioneer.com)
 * All rights reserved. Copyright (c) 2016 VECTIONEER.
 */
 
#ifndef SG_MONITOR_H
#define SG_MONITOR_H
 
#include "sg_monitorbase.h"
#include "sg_signalidgroup.h"
#include "utl_log.h"
 
#include <functional>
#include <algorithm>
#include <limits>
#include <vector>
#include <queue>
 
namespace mcx {
 
namespace signal_monitor {
 
template<typename t>
struct bool_stub { // bug in stl, containers treat bool as bit field
  using type = t;
};
 
template<>
struct bool_stub<bool> {
  using type = char;
};
 
template<typename T, typename CompareFunctor = std::equal_to<T>>
class SignalMonitor : public SignalMonitorBase {
 
  struct signal_data {
    bool enabled;
    TriggerType trigger_type;
    bool changed;
    double start_time_s;
    double actual_time_s;
    double stop_time_s;
    CompareFunctor cmp;
    bool input_ref_is_local;
    std::reference_wrapper<T> input_ref;
    T objective;
    std::function<bool(void)> clb;
  };
 
  T dummy;
 
  class SignalDataFactory {
  public:
 
    static signal_data make(const SignalId& signal_id, T& input_ref,
                            T objective_val, std::function<bool()> callback,
                            TriggerType trigger_type, double start_time_s,
                            double stop_time_s, bool input_ref_is_local) {
 
      signal_data sd = {.enabled = true, .trigger_type = trigger_type,
          .changed = true, .start_time_s = start_time_s, .actual_time_s = 0,
          .stop_time_s = stop_time_s, .cmp =
              {},
          .input_ref_is_local = input_ref_is_local, .input_ref = input_ref,
          .objective = objective_val, .clb = callback};
 
      return sd;
    }
 
    static signal_data make(const SignalId& signal_id, T& input_ref,
                            T objective_val, std::function<bool(const SignalId&)> callback,
                            TriggerType trigger_type, double start_time_s,
                            double stop_time_s, bool input_ref_is_local) {
 
      auto clb = std::bind(callback, signal_id);
      signal_data sd = {.enabled = true, .trigger_type = trigger_type,
          .changed = true, .start_time_s = start_time_s, .actual_time_s = 0,
          .stop_time_s = stop_time_s, .cmp =
              {},
          .input_ref_is_local = input_ref_is_local, .input_ref = input_ref,
          .objective = objective_val, .clb = clb};
 
      return sd;
    }
 
    static signal_data make(const SignalId& signal_id, T& input_ref,
                            T objective_val, std::function<bool(const SignalId&, T)> callback,
                            TriggerType trigger_type, double start_time_s, double stop_time_s,
                            bool input_ref_is_local) {
 
      auto clb = std::bind(callback, signal_id, std::ref(input_ref));
      signal_data sd = {.enabled = true, .trigger_type = trigger_type,
          .changed = true, .start_time_s = start_time_s, .actual_time_s = 0,
          .stop_time_s = stop_time_s, .cmp =
              {}, .input_ref_is_local = input_ref_is_local, .input_ref = input_ref,
          .objective = objective_val, .clb = clb};
 
      return sd;
    }
 
    template<typename U>
    static signal_data make(const SignalId& signal_id, T& input_ref,
                            T objective_val, std::pair<std::function<bool(U*)>, U*
 
    > callback,
                            TriggerType trigger_type,
                            double start_time_s,
                            double stop_time_s,
                            bool input_ref_is_local
    ) {
 
      auto clb = std::bind(callback.first, callback.second);
      signal_data sd = {.enabled = true, .trigger_type = trigger_type,
          .changed = true, .start_time_s = start_time_s, .actual_time_s = 0,
          .stop_time_s = stop_time_s, .cmp =
              {}, .input_ref_is_local = input_ref_is_local, .input_ref = input_ref,
          .objective = objective_val, .clb = clb};
 
      return sd;
    }
 
    template<typename U>
    static signal_data make(const SignalId& signal_id, T& input_ref,
                            T objective_val,
                            std::pair<std::function<bool(U*, const SignalId& signal_id)>, U*> callback,
                            TriggerType trigger_type, double start_time_s, double stop_time_s,
                            bool input_ref_is_local) {
 
      auto clb = std::bind(callback.first, callback.second, signal_id);
      signal_data sd = {.enabled = true, .trigger_type = trigger_type,
          .changed = true, .start_time_s = start_time_s, .actual_time_s = 0,
          .stop_time_s = stop_time_s, .cmp =
              {}, .input_ref_is_local = input_ref_is_local, .input_ref = input_ref,
          .objective = objective_val, .clb = clb};
 
      return sd;
    }
 
    template<typename U>
    static signal_data make(const SignalId& signal_id, T& input_ref,
                            T objective_val, std::pair<std::function<bool(U*, const SignalId&, T)>, U*> callback,
                            TriggerType trigger_type, double start_time_s, double stop_time_s,
                            bool input_ref_is_local) {
 
      auto clb = std::bind(callback.first, callback.second, signal_id,
                           std::ref(input_ref));
      signal_data sd = {.enabled = true, .trigger_type = trigger_type,
          .changed = true, .start_time_s = start_time_s, .actual_time_s = 0,
          .stop_time_s = stop_time_s, .cmp =
              {}, .input_ref_is_local = input_ref_is_local, .input_ref = input_ref,
          .objective = objective_val, .clb = clb};
 
      return sd;
    }
  };
 
  std::queue<typename bool_stub<T>::type> values_storage_;
  using callback_data = std::pair<SignalId, signal_data>;
  std::vector<callback_data> callback_vector_;
 
  void addToVector(const SignalId& signal_id, signal_data&& clb_data) {
    // always keep array sorted, to optimize number of iterations for search
    typename std::vector<callback_data>::iterator itr =
        std::find_if(callback_vector_.begin(), callback_vector_.end(),
                     [&](const auto& obj) {
                       return obj.first >= signal_id;
                     });
    callback_vector_.emplace(itr, std::make_pair(signal_id, clb_data));
  }
 
  bool isInRange(const signal_data& data) const {
    return (data.start_time_s <= data.actual_time_s) &&
           (data.actual_time_s < data.stop_time_s);
  }
 
  template<typename Action>
  void seekAndDestroy(const SignalIdGroup& signal_id_group, const Action& action) {
    auto new_err_itr = signal_id_group.begin();
    auto new_err_end = signal_id_group.end();
 
    auto err_itr = callback_vector_.begin();
    auto prev_itr = err_itr;
    auto err_end = callback_vector_.end();
 
    // signal_id_group is sorted and callback vector is sorted
    // callback might have multiple instances of a searched key
    for (; new_err_itr != new_err_end; ++new_err_itr) {
      if (err_itr == err_end) {
        err_itr = prev_itr;
      }
      while ((err_itr != err_end) && (err_itr->first <= *new_err_itr)) {
        err_itr = std::find_if(err_itr, err_end, [&](const auto& obj) {
          return obj.first == *new_err_itr;
        });
        if (err_itr != err_end) {
          action(err_itr->second);
          ++err_itr;
          prev_itr = err_itr;
        }
      }
    }
  }
 
public:
  // adding base class methods, otherwise they are not visible to a compiler
  using SignalMonitorBase::set;
  using SignalMonitorBase::reset;
  using SignalMonitorBase::enable;
  using SignalMonitorBase::disable;
 
  SignalMonitor(void) : callback_vector_() {
  }
 
  virtual ~SignalMonitor(void) {
 
  }
 
  void add(const SignalId& signal_id, T& input_ref, T objective_val,
           std::function<bool()> callback, TriggerType trigger_type = TRIGGER_ALWAYS,
           double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    addToVector(signal_id, SignalDataFactory::make(signal_id, input_ref,
                                                   objective_val, callback, trigger_type, start_time_s, stop_time_s,
                                                   false));
 
  }
 
  void add(const SignalId& signal_id, T& input_ref, T objective_val,
           std::function<bool(const SignalId& signal_id)> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    addToVector(signal_id, SignalDataFactory::make(signal_id, input_ref,
                                                   objective_val, callback, trigger_type, start_time_s, stop_time_s,
                                                   false));
 
  }
 
  void add(const SignalId& signal_id, T& input_ref, T objective_val,
           std::function<bool(const SignalId& signal_id, T)> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    addToVector(signal_id, SignalDataFactory::make(signal_id, input_ref,
                                                   objective_val, callback, trigger_type, start_time_s, stop_time_s,
                                                   false));
 
  }
 
  template<typename U>
  void add(const SignalId& signal_id, T& input_ref, T objective_val,
           std::pair<std::function<bool(U*)>, U*> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS,
           double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()
  ) {
 
    addToVector(signal_id, SignalDataFactory::make(signal_id, input_ref,
                                                   objective_val, callback, trigger_type, start_time_s, stop_time_s,
                                                   false));
 
  }
 
  template<typename U>
  void add(const SignalId& signal_id, T& input_ref, T objective_val,
           std::pair<std::function<bool(U*, const SignalId& signal_id)>, U*> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    addToVector(signal_id, SignalDataFactory::make(signal_id, input_ref,
                                                   objective_val, callback, trigger_type, start_time_s, stop_time_s,
                                                   false));
 
  }
 
  template<typename U>
  void add(const SignalId& signal_id, T& input_ref, T objective_val,
           std::pair<std::function<bool(U*, const SignalId& signal_id, T)>, U*> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    addToVector(signal_id, SignalDataFactory::make(signal_id, input_ref,
                                                   objective_val, callback, trigger_type, start_time_s, stop_time_s,
                                                   false));
 
  }
 
  //
 
  void add(const SignalId& signal_id, const T&& input_val, T objective_val,
           std::function<bool()> callback, TriggerType trigger_type = TRIGGER_ALWAYS,
           double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    values_storage_.push(std::move(input_val));
    addToVector(signal_id, SignalDataFactory::make(signal_id,
                                                   reinterpret_cast<T&> (values_storage_.back()), objective_val,
                                                   callback,
                                                   trigger_type, start_time_s, stop_time_s, true));
 
  }
 
  void add(const SignalId& signal_id, const T&& input_val, T objective_val,
           std::function<bool(const SignalId& signal_id)> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    values_storage_.push(std::move(input_val));
    addToVector(signal_id, SignalDataFactory::make(signal_id,
                                                   reinterpret_cast<T&> (values_storage_.back()), objective_val,
                                                   callback,
                                                   trigger_type, start_time_s, stop_time_s, true));
 
  }
 
  void add(const SignalId& signal_id, const T&& input_val, T objective_val,
           std::function<bool(const SignalId& signal_id, T)> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    values_storage_.push(std::move(input_val));
    addToVector(signal_id, SignalDataFactory::make(signal_id,
                                                   reinterpret_cast<T&> (values_storage_.back()), objective_val,
                                                   callback,
                                                   trigger_type = TRIGGER_ALWAYS, start_time_s, stop_time_s, true));
 
  }
 
  template<typename U>
  void add(const SignalId& signal_id, const T&& input_val, T objective_val,
           std::pair<std::function<bool(U*)>, U*> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS,
           double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()
  ) {
 
    values_storage_.push(std::move(input_val));
    addToVector(signal_id, SignalDataFactory::make(signal_id,
                                                   reinterpret_cast<T&> (values_storage_.back()), objective_val,
                                                   callback,
                                                   trigger_type, start_time_s, stop_time_s, true));
 
  }
 
  template<typename U>
  void add(const SignalId& signal_id, const T&& input_val, T objective_val,
           std::pair<std::function<bool(U*, const SignalId& signal_id)>, U*> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    values_storage_.push(std::move(input_val));
    addToVector(signal_id, SignalDataFactory::make(signal_id,
                                                   reinterpret_cast<T&> (values_storage_.back()), objective_val,
                                                   callback,
                                                   trigger_type, start_time_s, stop_time_s, true));
 
  }
 
  template<typename U>
  void add(const SignalId& signal_id, const T&& input_val, T objective_val,
           std::pair<std::function<bool(U*, const SignalId& signal_id, T)>, U*> callback,
           TriggerType trigger_type = TRIGGER_ALWAYS, double start_time_s = 0,
           double stop_time_s = std::numeric_limits<double>::max()) {
 
    values_storage_.push(std::move(input_val));
    addToVector(signal_id, SignalDataFactory::make(signal_id,
                                                   reinterpret_cast<T&> (values_storage_.back()), objective_val,
                                                   callback,
                                                   trigger_type, start_time_s, stop_time_s, true));
 
  }
 
  void iterate(double dt_sec);
 
  void set(const SignalId& signal_id);
 
  void set(const SignalId& signal_id, const T& value);
 
  void setObjective(const SignalId& signal_id, const T& value);
 
  void reset(const SignalId& signal_id);
 
  void enable(const SignalId& signal_id);
 
  void disable(const SignalId& signal_id);
 
  void set(const SignalIdGroup& signal_id_group);
 
  void reset(const SignalIdGroup& signal_id_group);
 
  void enable(const SignalIdGroup& signal_id_group);
 
  void disable(const SignalIdGroup& signal_id_group);
 
  void reset(void);
 
  void enable(void);
 
  void disable(void);
 
};
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::iterate(double dt_sec) {
  for (auto& a : callback_vector_) {
    auto& signal_data = a.second;
    if (signal_data.enabled) {
      bool signal_cmp = signal_data.cmp(signal_data.input_ref, signal_data.objective);
      if (isInRange(signal_data) && signal_cmp) {
        switch (signal_data.trigger_type) {
          case TriggerType::TRIGGER_ONCE:
            if (signal_data.changed) {
              signal_data.changed = !signal_data.clb();
            }
            break;
          case TriggerType::TRIGGER_TOGGLE:
            if (signal_data.clb()) {
              signal_data.objective = !signal_data.input_ref;
            }
            break;
          case TriggerType::TRIGGER_ALWAYS:
            signal_data.clb();
            break;
          default:
            log_assert(0, "TriggerType has unknown value!");
        }
      }
      if (signal_cmp) {
        signal_data.actual_time_s += dt_sec;
      } else {
        signal_data.changed = true;
        signal_data.actual_time_s = 0;
      }
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::set(const SignalId& signal_id) {
  for (auto& itr : callback_vector_) {
    if (itr.first == signal_id) {
      itr.second.input_ref.get() = itr.second.objective;
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::set(const SignalId& signal_id, const T& value) {
  for (auto& itr : callback_vector_) {
    if (itr.first == signal_id) {
      itr.second.input_ref.get() = value;
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::setObjective(const SignalId& signal_id, const T& value) {
  for (auto& itr : callback_vector_) {
    if (itr.first == signal_id) {
      itr.second.objective = value;
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::reset(const SignalId& signal_id) {
  for (auto& itr : callback_vector_) {
    if (itr.first == signal_id) {
      itr.second.changed = true;
      itr.second.actual_time_s = 0;
      if (itr.second.input_ref_is_local) {
        itr.second.input_ref.get() = !itr.second.objective;
      }
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::enable(const SignalId& signal_id) {
  for (auto& itr : callback_vector_) {
    if (itr.first == signal_id) {
      itr.second.enabled = true;
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::disable(const SignalId& signal_id) {
  for (auto& itr : callback_vector_) {
    if (itr.first == signal_id) {
      itr.second.enabled = false;
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::set(const SignalIdGroup& signal_id_group) {
  auto action = [](auto& signal) {
    signal.input_ref.get() = signal.objective;
  };
  seekAndDestroy(signal_id_group, action);
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::reset(const SignalIdGroup& signal_id_group) {
  auto action = [](auto& signal) {
    signal.changed = true;
    signal.actual_time_s = 0;
    if (signal.input_ref_is_local) {
      signal.input_ref.get() = !signal.objective;
    }
  };
  seekAndDestroy(signal_id_group, action);
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::enable(const SignalIdGroup& signal_id_group) {
  auto action = [](auto& signal) {
    signal.enabled = true;
  };
  seekAndDestroy(signal_id_group, action);
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::disable(const SignalIdGroup& signal_id_group) {
  auto action = [](auto& signal) -> void {
    signal.enabled = false;
  };
  seekAndDestroy(signal_id_group, action);
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::reset(void) {
  for (auto& itr : callback_vector_) {
    itr.second.changed = true;
    itr.second.actual_time_s = 0;
    if (itr.second.input_ref_is_local) {
      itr.second.input_ref.get() = !itr.second.objective;
    }
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::enable(void) {
  for (auto& itr : callback_vector_) {
    itr.second.enabled = true;
  }
}
 
template<typename T, typename CompareFunctor>
void SignalMonitor<T, CompareFunctor>::disable(void) {
  for (auto& itr : callback_vector_) {
    itr.second.enabled = false;
  }
}
 
}  // namespace signal_monitor
 
}  // namespace mcx
 
#endif /* SG_MONITOR_H */