#include "include/Motors.hpp"
#include "include/spi.hpp"
#include "include/Robot.hpp"
#include <spdlog/spdlog.h>
#include <limits>

void Motors::set_power(uint8_t port, double percent) {
  if (fabs(percent) > 100.0) {
    spdlog::error("Invalid motor percent specified. Should be -100 <= percent <= 100", percent);
  }

  Mode mode = Mode::COAST;
  if (percent < 0.0) {
    mode = Mode::BACKWARD;
  } else if (percent > 0.0) {
    mode = Mode::FORWARD;
  }

  auto pwm = (uint16_t) (fabs(percent) * (std::numeric_limits<uint16_t>::max() / 100.0));
  set_pwm(port, pwm, mode);
}

void Motors::set_pwm(uint8_t port, uint16_t pwm, Mode mode) {
  if (port >= ROBOT_MOTOR_COUNT) {
    spdlog::error("Invalid motor port {} specified", port);
  }

  Spi::getInstance().write(Spi::MOTOR_1_PWM_H + port * 2, 2, pwm);
  Spi::getInstance().write(Spi::PWM_1_CTRL + port, 1, mode);
}

