#include "rc_pwm_control.h"

#include "device_drive.h"
#include "device_model.h"
#include "devices/device_1101.h"
#include "devices/device_1102.h"
#include "devices/device_1201.h"
#include "esp_log.h"
#include "sdkconfig.h"
#include <stdbool.h>

static const char *tag_s = "RC_PWM_CTRL";
static const device_drive_ops_t *drive_ops_s = NULL;

typedef struct {
    float kp;
    float ki;
    float kd;
    float out_min;
    float out_max;
    float i_min;
    float i_max;
    float integral;
    float prev_error;
    bool enabled;
    rc_pid_actuator_t actuator;
    bool configured;
} rc_pid_ctx_t;

static rc_pid_ctx_t pid_ctx_s[RC_PID_CH_MAX];

static float clampf(float v, float lo, float hi)
{
    if (v < lo) {
        return lo;
    }
    if (v > hi) {
        return hi;
    }
    return v;
}

static ledc_channel_t rc_pwm_channel_from_gpio(int gpio)
{
    switch (gpio) {
    case RC_PWM_PIN_DRV1_A:
        return LEDC_CHANNEL_0;
    case RC_PWM_PIN_DRV2_A:
        return LEDC_CHANNEL_1;
    case RC_PWM_PIN_DRV2_B:
        return LEDC_CHANNEL_2;
    case RC_PWM_PIN_DRV1_B:
        return LEDC_CHANNEL_3;
    case RC_PWM_PIN_AUX_15:
        return LEDC_CHANNEL_4;
    case RC_PWM_PIN_AUX_16:
        return LEDC_CHANNEL_5;
    default:
        return LEDC_CHANNEL_MAX;
    }
}

static rc_aux_role_t rc_pwm_aux_role_of_pin(int gpio)
{
    if (gpio == RC_PWM_PIN_AUX_15) {
#if CONFIG_APP_PWM_IO15_ESC
        return RC_AUX_ROLE_ESC;
#else
        return RC_AUX_ROLE_SERVO;
#endif
    }
    if (gpio == RC_PWM_PIN_AUX_16) {
#if CONFIG_APP_PWM_IO16_ESC
        return RC_AUX_ROLE_ESC;
#else
        return RC_AUX_ROLE_SERVO;
#endif
    }
    return RC_AUX_ROLE_ESC;
}

static void rc_pwm_set_duty_by_gpio(int gpio, uint32_t duty)
{
    ledc_channel_t ch = rc_pwm_channel_from_gpio(gpio);
    if (ch == LEDC_CHANNEL_MAX) {
        return;
    }
    ledc_set_duty(RC_PWM_LEDC_MODE, ch, duty);
    ledc_update_duty(RC_PWM_LEDC_MODE, ch);
}

static uint32_t rc_pwm_angle_to_duty(uint32_t angle)
{
    if (angle > 180U) {
        angle = 180U;
    }
    return RC_PWM_DUTY_500US + (angle * (RC_PWM_DUTY_2500US - RC_PWM_DUTY_500US) / 180U);
}

static uint32_t rc_pwm_esc_percent_to_duty(uint32_t percent)
{
    if (percent > 100U) {
        percent = 100U;
    }
    return RC_ESC_DUTY_MIN + (RC_ESC_DUTY_MAX - RC_ESC_DUTY_MIN) * percent / 100U;
}

static void rc_pid_apply_output(rc_pid_actuator_t actuator, float output)
{
    switch (actuator) {
    case RC_PID_ACT_ESC_AUX_BOTH:
        rc_pwm_set_dual_esc_percent((uint32_t)clampf(output, 0.0f, 100.0f));
        break;
    case RC_PID_ACT_DRV1_A:
        rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, (uint32_t)clampf(output, 0.0f, 100.0f));
        break;
    case RC_PID_ACT_DRV1_B:
        rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_B, (uint32_t)clampf(output, 0.0f, 100.0f));
        break;
    case RC_PID_ACT_DRV2_A:
        rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, (uint32_t)clampf(output, 0.0f, 100.0f));
        break;
    case RC_PID_ACT_DRV2_B:
        rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_B, (uint32_t)clampf(output, 0.0f, 100.0f));
        break;
    case RC_PID_ACT_STEERING_SERVO:
        rc_pwm_set_steering_angle_deg((uint32_t)clampf(output, 0.0f, 180.0f));
        break;
    default:
        break;
    }
}

esp_err_t rc_pwm_control_init(void)
{
    const ledc_timer_config_t ledc_timer = {
        .speed_mode = RC_PWM_LEDC_MODE,
        .timer_num = RC_PWM_LEDC_TIMER,
        .duty_resolution = RC_PWM_DUTY_RES,
        .freq_hz = RC_PWM_FREQ_HZ,
        .clk_cfg = LEDC_AUTO_CLK,
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    const uint32_t init_15 = (rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_15) == RC_AUX_ROLE_SERVO) ? RC_PWM_DUTY_1500US : RC_ESC_DUTY_MID;
    const uint32_t init_16 = (rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_16) == RC_AUX_ROLE_SERVO) ? RC_PWM_DUTY_1500US : RC_ESC_DUTY_MID;

    const ledc_channel_config_t channels[] = {
        {.gpio_num = RC_PWM_PIN_DRV1_A, .speed_mode = RC_PWM_LEDC_MODE, .channel = LEDC_CHANNEL_0, .intr_type = LEDC_INTR_DISABLE, .timer_sel = RC_PWM_LEDC_TIMER, .duty = 0, .hpoint = 0},
        {.gpio_num = RC_PWM_PIN_DRV2_A, .speed_mode = RC_PWM_LEDC_MODE, .channel = LEDC_CHANNEL_1, .intr_type = LEDC_INTR_DISABLE, .timer_sel = RC_PWM_LEDC_TIMER, .duty = 0, .hpoint = 0},
        {.gpio_num = RC_PWM_PIN_DRV2_B, .speed_mode = RC_PWM_LEDC_MODE, .channel = LEDC_CHANNEL_2, .intr_type = LEDC_INTR_DISABLE, .timer_sel = RC_PWM_LEDC_TIMER, .duty = 0, .hpoint = 0},
        {.gpio_num = RC_PWM_PIN_DRV1_B, .speed_mode = RC_PWM_LEDC_MODE, .channel = LEDC_CHANNEL_3, .intr_type = LEDC_INTR_DISABLE, .timer_sel = RC_PWM_LEDC_TIMER, .duty = 0, .hpoint = 0},
        {.gpio_num = RC_PWM_PIN_AUX_15, .speed_mode = RC_PWM_LEDC_MODE, .channel = LEDC_CHANNEL_4, .intr_type = LEDC_INTR_DISABLE, .timer_sel = RC_PWM_LEDC_TIMER, .duty = init_15, .hpoint = 0},
        {.gpio_num = RC_PWM_PIN_AUX_16, .speed_mode = RC_PWM_LEDC_MODE, .channel = LEDC_CHANNEL_5, .intr_type = LEDC_INTR_DISABLE, .timer_sel = RC_PWM_LEDC_TIMER, .duty = init_16, .hpoint = 0},
    };

    for (int i = 0; i < (int)(sizeof(channels) / sizeof(channels[0])); ++i) {
        ESP_ERROR_CHECK(ledc_channel_config(&channels[i]));
    }

    if (!drive_ops_s) {
        drive_ops_s = device_1201_get_ops();
    }
    ESP_LOGI(tag_s, "PWM init done: D1(10/21) D2(11/12) AUX(15:%s 16:%s) 50Hz profile=%s",
             rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_15) == RC_AUX_ROLE_SERVO ? "servo" : "esc",
             rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_16) == RC_AUX_ROLE_SERVO ? "servo" : "esc",
             drive_ops_s->name);
    return ESP_OK;
}

void rc_pwm_control_set_drive_from_device_id(const char *device_id)
{
    const device_model_t m = device_model_from_full_id(device_id);
    if (device_model_is_1101(m)) {
        drive_ops_s = device_1101_get_ops();
    } else if (device_model_is_1102(m)) {
        drive_ops_s = device_1102_get_ops();
#if CONFIG_APP_PWM_IO16_ESC
        ESP_LOGW(tag_s, "1102 需要 IO16 作为舵机，请在 menuconfig 将 GPIO16 角色改为 SERVO");
#endif
    } else {
        drive_ops_s = device_1201_get_ops();
    }
    ESP_LOGI(tag_s, "当前设备策略: %s", drive_ops_s->name);
}

void rc_vehicle_stop(void)
{
    if (drive_ops_s && drive_ops_s->stop) {
        drive_ops_s->stop();
    }
}

void rc_vehicle_shot(int pin, int val)
{
    if (drive_ops_s && drive_ops_s->shot) {
        drive_ops_s->shot(pin, val);
    }
}

void rc_vehicle_control(int mode, int val)
{
    if (drive_ops_s && drive_ops_s->control) {
        drive_ops_s->control(mode, val);
    }
}

void rc_pwm_set_drive_percent(int gpio, uint32_t duty_percent)
{
    if (duty_percent > 100U) {
        duty_percent = 100U;
    }
    if (gpio != RC_PWM_PIN_DRV1_A && gpio != RC_PWM_PIN_DRV1_B &&
        gpio != RC_PWM_PIN_DRV2_A && gpio != RC_PWM_PIN_DRV2_B) {
        return;
    }
    const uint32_t duty = (duty_percent * 8191U) / 100U;
    rc_pwm_set_duty_by_gpio(gpio, duty);
}

void rc_pwm_stop_all_drive_outputs(void)
{
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_B, 0);
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, 0);
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_B, 0);
}

void rc_pwm_set_aux_servo_angle_deg(int gpio, uint32_t angle)
{
    if (gpio != RC_PWM_PIN_AUX_15 && gpio != RC_PWM_PIN_AUX_16) {
        return;
    }
    if (rc_pwm_aux_role_of_pin(gpio) != RC_AUX_ROLE_SERVO) {
        return;
    }
    rc_pwm_set_duty_by_gpio(gpio, rc_pwm_angle_to_duty(angle));
}

void rc_pwm_set_aux_esc_percent(int gpio, uint32_t percent)
{
    if (gpio != RC_PWM_PIN_AUX_15 && gpio != RC_PWM_PIN_AUX_16) {
        return;
    }
    if (rc_pwm_aux_role_of_pin(gpio) != RC_AUX_ROLE_ESC) {
        return;
    }
    rc_pwm_set_duty_by_gpio(gpio, rc_pwm_esc_percent_to_duty(percent));
}

void rc_pwm_set_dual_esc_percent(uint32_t percent)
{
    if (rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_15) == RC_AUX_ROLE_ESC) {
        rc_pwm_set_aux_esc_percent(RC_PWM_PIN_AUX_15, percent);
    }
    if (rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_16) == RC_AUX_ROLE_ESC) {
        rc_pwm_set_aux_esc_percent(RC_PWM_PIN_AUX_16, percent);
    }
}

void rc_pwm_set_steering_angle_deg(uint32_t angle)
{
    if (rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_15) == RC_AUX_ROLE_SERVO) {
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_15, angle);
        return;
    }
    if (rc_pwm_aux_role_of_pin(RC_PWM_PIN_AUX_16) == RC_AUX_ROLE_SERVO) {
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, angle);
    }
}

esp_err_t rc_pid_config(rc_pid_channel_t ch, const rc_pid_config_t *cfg)
{
    if (cfg == NULL || ch < RC_PID_CH_0 || ch >= RC_PID_CH_MAX) {
        return ESP_ERR_INVALID_ARG;
    }
    if (cfg->out_min >= cfg->out_max || cfg->i_min >= cfg->i_max) {
        return ESP_ERR_INVALID_ARG;
    }

    rc_pid_ctx_t *ctx = &pid_ctx_s[ch];
    ctx->kp = cfg->kp;
    ctx->ki = cfg->ki;
    ctx->kd = cfg->kd;
    ctx->out_min = cfg->out_min;
    ctx->out_max = cfg->out_max;
    ctx->i_min = cfg->i_min;
    ctx->i_max = cfg->i_max;
    ctx->enabled = cfg->enabled;
    ctx->actuator = cfg->actuator;
    ctx->integral = 0.0f;
    ctx->prev_error = 0.0f;
    ctx->configured = true;
    return ESP_OK;
}

void rc_pid_reset(rc_pid_channel_t ch)
{
    if (ch < RC_PID_CH_0 || ch >= RC_PID_CH_MAX) {
        return;
    }
    pid_ctx_s[ch].integral = 0.0f;
    pid_ctx_s[ch].prev_error = 0.0f;
}

void rc_pid_enable(rc_pid_channel_t ch, bool enabled)
{
    if (ch < RC_PID_CH_0 || ch >= RC_PID_CH_MAX) {
        return;
    }
    pid_ctx_s[ch].enabled = enabled;
}

float rc_pid_step(rc_pid_channel_t ch,
                  float target,
                  float feedback,
                  float dt_s,
                  bool apply_output)
{
    if (ch < RC_PID_CH_0 || ch >= RC_PID_CH_MAX || dt_s <= 0.0f) {
        return 0.0f;
    }

    rc_pid_ctx_t *ctx = &pid_ctx_s[ch];
    if (!ctx->configured || !ctx->enabled) {
        return 0.0f;
    }

    const float error = target - feedback;
    ctx->integral += error * dt_s;
    ctx->integral = clampf(ctx->integral, ctx->i_min, ctx->i_max);
    const float derivative = (error - ctx->prev_error) / dt_s;
    ctx->prev_error = error;

    float output = ctx->kp * error + ctx->ki * ctx->integral + ctx->kd * derivative;
    output = clampf(output, ctx->out_min, ctx->out_max);

    if (apply_output) {
        rc_pid_apply_output(ctx->actuator, output);
    }
    return output;
}
