#include <eqpalg/utility/entCentExit.h>
#include <functional>

using std::map;
extern const int TRUEMAXTIME;
std::function<bool(double, double, double)> Speed_is_stable =
    [](double a, double b, double c) { return std::fabs(a - b) < c; };
void stableDetector(double v1, double v2, double th, int& times,
                    bool& isStable) {
  if (!isStable) {
    if (Speed_is_stable(v1, v2, th)) {
      if (times < TRUEMAXTIME) {
        times++;
      } else {
        isStable = true;
      }
    } else {
      times = 0;
    }
  } else {
    if (!Speed_is_stable(v1, v2, th)) {
      times = 0;
      isStable = false;
    }
  }
}

void entCentExit::statusDetection() {
  double centSpeedSet =
      SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
                                                              cent_speed_set];
  double centSpeedAct =
      SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
                                                              cent_speed_act];
  double entSpeed =
      SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
                                                              ent_speed_act];
  double exitSpeed =
      SingletonTemplate<GlobaltemSharedMemory>::GetInstance()[prefix +
                                                              exit_speed_act];
  //中央速度平稳性
  stableDetector(centSpeedSet, centSpeedAct, 1, cent_times_OK_,
                 cent_speed_stable_);
  //入口速度平稳性
  stableDetector(entSpeed, centSpeedAct, 2, ent_times_OK_,
                 ent_speed_stable_);
  //出口速度平稳性
  stableDetector(exitSpeed, centSpeedAct, 2, exit_times_OK_,
                 exit_speed_stable_);

  if (ECEqueue_.size() < TRUEMAXTIME) {
    ECEqueue_.push(ECELabel(cent_speed_stable_, ent_speed_stable_, exit_speed_stable_));
  } else {
    auto ihd_ece = ECEqueue_.front();
    cent_speed_stable_last_ = ihd_ece.cent_speed_stable;
    ent_speed_stable_last_ = ihd_ece.ent_speed_stable;
    exit_speed_stable_last_ = ihd_ece.exit_speed_stable;
    ECEqueue_.pop();
  }

  // auto nowTime = std::chrono::system_clock::now();
  // if (nowTime - before_five_seconds_ > 5s) {
  //   before_five_seconds_ = nowTime;
  //   bool cent_speed_stable_last_ = cent_speed_stable_;
  //   bool ent_speed_stable_last_ = ent_speed_stable_;
  //   bool exit_speed_stable_last_ = exit_speed_stable_;
  // }
}