#include "ugv_io.hh"

#include <driver/gpio.h>
#include <driver/mcpwm.h>
#include <esp_log.h>

#include <cmath>

namespace ugv {
namespace io {

static const char *TAG = "ugv_io";

#define ERROR_CHECK(ret)                                            \
  {                                                                 \
    esp_err_t _error_code = (ret);                                  \
    if (_error_code != ESP_OK) {                                    \
      const char *_error_name = esp_err_to_name(_error_code);       \
      ESP_LOGE(TAG, "%s:%d error: %s (%d)", __FUNCTION__, __LINE__, \
               _error_name, _error_code);                           \
      return;                                                       \
    }                                                               \
  }

static constexpr mcpwm_unit_t MCPWM_UNIT = MCPWM_UNIT_0;
static constexpr gpio_num_t MOTOR_LEFT_PWM = GPIO_NUM_2;
static constexpr gpio_num_t MOTOR_LEFT_DIR = GPIO_NUM_0;
static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_12;
static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_16;

IOClass IO;

IOClass::IOClass() {}

IOClass::~IOClass() {}

void IOClass::Init() {
  InitMotors();
  mpu_.Init();
  gps_.Init();
}

void IOClass::InitMotors() {
  ERROR_CHECK(gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT));
  ERROR_CHECK(gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT));
  ERROR_CHECK(gpio_set_pull_mode(MOTOR_LEFT_DIR, GPIO_FLOATING));
  ERROR_CHECK(gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT));
  ERROR_CHECK(gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT));
  ERROR_CHECK(gpio_set_pull_mode(MOTOR_RIGHT_DIR, GPIO_FLOATING));
  ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM));
  ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM1A, MOTOR_RIGHT_PWM));

  mcpwm_config_t mcpwm_config;
  mcpwm_config.frequency = 20000;  // 20khz
  mcpwm_config.cmpr_a = 0;
  mcpwm_config.cmpr_b = 0;
  mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER;  // for symmetric pwm
  mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0;         // active high
  ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config));
  ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_1, &mcpwm_config));
}

void IOClass::ReadInputs(Inputs &inputs) {
  gps_.GetData(inputs.gps);
  mpu_.GetData(inputs.mpu);
}

void IOClass::WriteOutputs(const Outputs &outputs) {
  esp_err_t ret;
  ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
                       fabsf(outputs.left_motor * 100.0f));
  ERROR_CHECK(ret);
  ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_0);
  ERROR_CHECK(ret);
  ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_1, MCPWM_OPR_A,
                       fabsf(outputs.right_motor * 100.0f));
  ERROR_CHECK(ret);
  ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1);
  ERROR_CHECK(ret);
  bool left_dir, right_dir;
  left_dir = outputs.left_motor > 0.f;
  right_dir = outputs.right_motor < 0.f;

  ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir);
  ERROR_CHECK(ret);
  ret = gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
  ERROR_CHECK(ret);
  ESP_LOGV(TAG, "motor outputs: (%f, %f)", outputs.left_motor,
           outputs.right_motor);
}

};  // namespace io
};  // namespace ugv