#include "include/HealthChecker.hpp"
#include "include/ClosedLoopMotorController.hpp"
#include "include/spi.hpp"
#include "include/IRSensors.hpp"

HealthChecker::HealthChecker() {
  last_update = clock::now();
  check_thread = std::thread(&HealthChecker::check_loop, this);
  check_thread.detach();
}

void HealthChecker::update() {
  last_update = clock::now();
}

void HealthChecker::check_loop() {
  while (true) {
    if (clock::now() - last_update < std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS)) {
      Spi::getInstance().read_version();
      stop_sent = false;
    } else if (!stop_sent) {
      ClosedLoopMotorController::getInstance().stop_all();
      IRSensors::disable();
      stop_sent = true;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(HEALTH_CHECK_TIMEOUT_MS / 2));
  }
}
