#include "motors.h"

#include <stdlib.h>

#include "driver/ledc.h"
#include "esp_err.h"
#include "esp_log.h"

#define MOTOR_PWM_MODE LEDC_LOW_SPEED_MODE
#define MOTOR_PWM_TIMER LEDC_TIMER_0
#define MOTOR_PWM_FREQ_HZ 20000
#define MOTOR_PWM_RESOLUTION LEDC_TIMER_10_BIT
#define MOTOR_PWM_MAX_DUTY ((1 << 10) - 1)
#define MOTOR_MIN_VALUE 60

typedef struct {
    int fwd_pin;
    int bak_pin;
    ledc_channel_t fwd_channel;
    ledc_channel_t bak_channel;
} motor_config_t;

static const motor_config_t s_motor_configs[] = {
    [MOTOR1] = {
        .fwd_pin = MOTOR1_FWD_PIN,
        .bak_pin = MOTOR1_BAK_PIN,
        .fwd_channel = LEDC_CHANNEL_0,
        .bak_channel = LEDC_CHANNEL_1,
    },
    [MOTOR2] = {
        .fwd_pin = MOTOR2_FWD_PIN,
        .bak_pin = MOTOR2_BAK_PIN,
        .fwd_channel = LEDC_CHANNEL_2,
        .bak_channel = LEDC_CHANNEL_3,
    },
};

static uint32_t percentile_to_duty(percentile_t percentile)
{
    int clamped = percentile;
    const uint32_t min_duty = (MOTOR_PWM_MAX_DUTY * MOTOR_MIN_VALUE) / 100;
    if (clamped > 100) {
        clamped = 100;
    } else if (clamped < -100) {
        clamped = -100;
    }

    if (clamped == 0) {
        return 0;
    }

    return min_duty + (((uint32_t)abs(clamped) * (MOTOR_PWM_MAX_DUTY - min_duty)) / 100);
}

static void set_channel_duty(ledc_channel_t channel, uint32_t duty)
{
    ESP_ERROR_CHECK(ledc_set_duty(MOTOR_PWM_MODE, channel, duty));
    ESP_ERROR_CHECK(ledc_update_duty(MOTOR_PWM_MODE, channel));
}

void init_motors(void)
{
    const ledc_timer_config_t timer_config = {
        .speed_mode = MOTOR_PWM_MODE,
        .timer_num = MOTOR_PWM_TIMER,
        .duty_resolution = MOTOR_PWM_RESOLUTION,
        .freq_hz = MOTOR_PWM_FREQ_HZ,
        .clk_cfg = LEDC_AUTO_CLK,
    };

    ESP_ERROR_CHECK(ledc_timer_config(&timer_config));

    for (size_t i = 0; i < (sizeof(s_motor_configs) / sizeof(s_motor_configs[0])); ++i) {
        const motor_config_t *motor = &s_motor_configs[i];
        const ledc_channel_config_t fwd_channel_config = {
            .gpio_num = motor->fwd_pin,
            .speed_mode = MOTOR_PWM_MODE,
            .channel = motor->fwd_channel,
            .intr_type = LEDC_INTR_DISABLE,
            .timer_sel = MOTOR_PWM_TIMER,
            .duty = 0,
            .hpoint = 0,
        };
        const ledc_channel_config_t bak_channel_config = {
            .gpio_num = motor->bak_pin,
            .speed_mode = MOTOR_PWM_MODE,
            .channel = motor->bak_channel,
            .intr_type = LEDC_INTR_DISABLE,
            .timer_sel = MOTOR_PWM_TIMER,
            .duty = 0,
            .hpoint = 0,
        };

        ESP_ERROR_CHECK(ledc_channel_config(&fwd_channel_config));
        ESP_ERROR_CHECK(ledc_channel_config(&bak_channel_config));
    }

    set_motors(0, 0);
}

void set_motor(motor_t motor, percentile_t percentile)
{
    if (motor < MOTOR1 || motor > MOTOR2) {
        return;
    }

    const motor_config_t *config = &s_motor_configs[motor];
    const uint32_t duty = percentile_to_duty(percentile);

    if (percentile > 0) {
        set_channel_duty(config->bak_channel, 0);
        set_channel_duty(config->fwd_channel, duty);
    } else if (percentile < 0) {
        set_channel_duty(config->fwd_channel, 0);
        set_channel_duty(config->bak_channel, duty);
    } else {
        set_channel_duty(config->fwd_channel, 0);
        set_channel_duty(config->bak_channel, 0);
    }
}

void set_motors(percentile_t left, percentile_t right)
{
    set_motor(MOTOR1, left);
    set_motor(MOTOR2, right);
}