#include "include/IRSensors.hpp"
#include "include/spi.hpp"
#include "include/Robot.hpp"
#include "include/mathUtils.hpp"

void IRSensors::enable() {
  uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};
  std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 1);
  Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data);
}

void IRSensors::disable() {
  uint8_t data[ROBOT_IR_SENSOR_COUNT] = {};
  std::fill_n(data, ROBOT_IR_SENSOR_COUNT, 0);
  Spi::getInstance().write_array(Spi::IR_1_LED, ROBOT_IR_SENSOR_COUNT, data);
}


std::array<uint16_t, ROBOT_IR_SENSOR_COUNT> IRSensors::read() {
  if (cache.is_expired()) {
    uint8_t data[ROBOT_IR_SENSOR_COUNT * 2] = {0};
    Spi::getInstance().read_array(Spi::IR_1_H, 2 * ROBOT_IR_SENSOR_COUNT, data);
    for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) {
      cached_values.at(i) = mathUtils::from_bytes<uint16_t>(data + i * 2, 2);
    }
    cache.update();
  }
  return cached_values;
}