#include "ugv_io.h"

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

#define IO_MCPWM_UNIT MCPWM_UNIT_0
#define IO_MOTOR_LEFT_PWM 0
#define IO_MOTOR_LEFT_DIR 0
#define IO_MOTOR_RIGHT_PWM 0
#define IO_MOTOR_RIGHT_DIR 0

typedef struct ugv_io_s {
} ugv_io_t;

static ugv_io_t io;

void ugv_io_init() {
  gpio_set_direction(IO_MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT);
  gpio_set_direction(IO_MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT);
  gpio_set_direction(IO_MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT);
  gpio_set_direction(IO_MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT);
  mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0A, IO_MOTOR_LEFT_PWM);
  mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0B, IO_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
  mcpwm_init(IO_MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config);
}

void ugv_io_write_outputs(ugv_outputs_t *outputs) {
  mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
                 fabs(outputs->left_motor));
  mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B,
                 fabs(outputs->right_motor));
  bool left_dir, right_dir;
  left_dir  = outputs->left_motor < 0.f;
  right_dir = outputs->right_motor < 0.f;
  gpio_set_level(IO_MOTOR_LEFT_DIR, left_dir);
  gpio_set_level(IO_MOTOR_RIGHT_DIR, right_dir);
}