#include "device_1102_brake.h"

#include "core/task_manager.h"
#include "rc_pwm_control.h"

#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"

#include <stdbool.h>

static const char *tag_s = "DEV1102_BRK";

#define BRAKE_QUEUE_LEN            1
#define BRAKE_TICK_MS              6U
#define BRAKE_STOP_SPEED_TH        3.0f
#define BRAKE_DRIVE_PWM_MIN        28U
#define BRAKE_SPEED_LOW_TH         45U
#define BRAKE_SPEED_HIGH_TH        70U

typedef struct {
    float est_speed;
    float integral;
    float prev_error;
} dev1102_brake_pid_t;

typedef struct {
    uint32_t kick_duty_min;
    uint32_t kick_duty_max;
    uint32_t kick_ms_min;
    uint32_t kick_ms_max;
    float duty_max;
    float duty_floor_ratio;
    float pid_kp;
    float pid_ki;
    float pid_kd;
    uint32_t max_loop_ms;
} dev1102_brake_profile_t;

static QueueHandle_t s_brake_q;
static TaskHandle_t s_brake_task;
static volatile bool s_abort;

static void device_1102_brake_both(uint32_t duty_pct)
{
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, duty_pct);
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_B, duty_pct);
}

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

static void dev1102_brake_profile(uint32_t start_pct, dev1102_brake_profile_t *out)
{
    if (start_pct <= BRAKE_SPEED_LOW_TH) {
        /* 低速：强抱闸、短周期，尽快停住 */
        *out = (dev1102_brake_profile_t){
            .kick_duty_min = 72U,
            .kick_duty_max = 88U,
            .kick_ms_min = 35U,
            .kick_ms_max = 50U,
            .duty_max = 85.0f,
            .duty_floor_ratio = 0.72f,
            .pid_kp = 1.05f,
            .pid_ki = 0.22f,
            .pid_kd = 0.12f,
            .max_loop_ms = 220U,
        };
        return;
    }
    if (start_pct <= BRAKE_SPEED_HIGH_TH) {
        *out = (dev1102_brake_profile_t){
            .kick_duty_min = 62U,
            .kick_duty_max = 78U,
            .kick_ms_min = 45U,
            .kick_ms_max = 65U,
            .duty_max = 78.0f,
            .duty_floor_ratio = 0.62f,
            .pid_kp = 0.92f,
            .pid_ki = 0.18f,
            .pid_kd = 0.10f,
            .max_loop_ms = 320U,
        };
        return;
    }
    /* 高速：抱闸略柔和但 PID 持续施压，避免长距离滑行 */
    *out = (dev1102_brake_profile_t){
        .kick_duty_min = 58U,
        .kick_duty_max = 76U,
        .kick_ms_min = 60U,
        .kick_ms_max = 95U,
        .duty_max = 72.0f,
        .duty_floor_ratio = 0.50f,
        .pid_kp = 0.78f,
        .pid_ki = 0.14f,
        .pid_kd = 0.08f,
        .max_loop_ms = 450U,
    };
}

static uint32_t dev1102_brake_kick_duty(uint32_t start_pct, const dev1102_brake_profile_t *prof)
{
    if (start_pct == 0U) {
        return 55U;
    }
    if (start_pct <= BRAKE_DRIVE_PWM_MIN) {
        return prof->kick_duty_min;
    }
    return prof->kick_duty_min +
           (start_pct - BRAKE_DRIVE_PWM_MIN) * (prof->kick_duty_max - prof->kick_duty_min) /
               (100U - BRAKE_DRIVE_PWM_MIN);
}

static uint32_t dev1102_brake_kick_ms(uint32_t start_pct, const dev1102_brake_profile_t *prof)
{
    if (start_pct == 0U) {
        return 45U;
    }
    if (start_pct <= BRAKE_DRIVE_PWM_MIN) {
        return prof->kick_ms_min;
    }
    return prof->kick_ms_min +
           (start_pct - BRAKE_DRIVE_PWM_MIN) * (prof->kick_ms_max - prof->kick_ms_min) /
               (100U - BRAKE_DRIVE_PWM_MIN);
}

static float dev1102_brake_pid_step(dev1102_brake_pid_t *pid,
                                    float target,
                                    float dt_s,
                                    const dev1102_brake_profile_t *prof)
{
    /* 剩余速度越大，制动输出越大（此前符号反了导致 PID 阶段几乎不刹车） */
    const float error = pid->est_speed - target;
    pid->integral += error * dt_s;
    pid->integral = dev1102_clampf(pid->integral, -30.0f, 30.0f);
    const float derivative = (error - pid->prev_error) / dt_s;
    pid->prev_error = error;

    float out = prof->pid_kp * error + prof->pid_ki * pid->integral + prof->pid_kd * derivative;
    const float floor = pid->est_speed * prof->duty_floor_ratio;
    if (out < floor) {
        out = floor;
    }
    return dev1102_clampf(out, 0.0f, prof->duty_max);
}

static void dev1102_brake_model_step(dev1102_brake_pid_t *pid, float brake_duty_pct, float dt_s)
{
    const float b = brake_duty_pct / 100.0f;
    const float decel = (150.0f * b + 6.0f * (pid->est_speed / 100.0f)) * dt_s;
    pid->est_speed -= decel;
    if (pid->est_speed < 0.0f) {
        pid->est_speed = 0.0f;
    }
}

static void device_1102_brake_run(uint32_t start_pct)
{
    dev1102_brake_profile_t prof;
    dev1102_brake_profile(start_pct, &prof);

    const uint32_t kick_duty = dev1102_brake_kick_duty(start_pct, &prof);
    const uint32_t kick_ms = dev1102_brake_kick_ms(start_pct, &prof);
    const float dt_s = (float)BRAKE_TICK_MS / 1000.0f;

    dev1102_brake_pid_t pid = {
        .est_speed = (float)(start_pct == 0U ? 40U : start_pct),
        .integral = 0.0f,
        .prev_error = 0.0f,
    };

    ESP_LOGW(tag_s, "brake run start=%u kick=%u%% %ums",
             (unsigned)start_pct, (unsigned)kick_duty, (unsigned)kick_ms);

    device_1102_brake_both(kick_duty);
    uint32_t kick_elapsed = 0U;
    while (kick_elapsed < kick_ms) {
        if (s_abort) {
            return;
        }
        dev1102_brake_model_step(&pid, (float)kick_duty, dt_s);
        vTaskDelay(pdMS_TO_TICKS(BRAKE_TICK_MS));
        kick_elapsed += BRAKE_TICK_MS;
    }

    const TickType_t loop_start = xTaskGetTickCount();
    while (pid.est_speed > BRAKE_STOP_SPEED_TH) {
        if (s_abort) {
            return;
        }
        if ((xTaskGetTickCount() - loop_start) > pdMS_TO_TICKS(prof.max_loop_ms)) {
            ESP_LOGW(tag_s, "brake timeout est=%.1f start=%u", pid.est_speed, (unsigned)start_pct);
            device_1102_brake_both((uint32_t)(prof.duty_max + 0.5f));
            vTaskDelay(pdMS_TO_TICKS(30U));
            break;
        }

        const float brake_duty = dev1102_brake_pid_step(&pid, 0.0f, dt_s, &prof);
        device_1102_brake_both((uint32_t)(brake_duty + 0.5f));
        dev1102_brake_model_step(&pid, brake_duty, dt_s);
        vTaskDelay(pdMS_TO_TICKS(BRAKE_TICK_MS));
    }

    device_1102_brake_both(0U);
    ESP_LOGI(tag_s, "brake done start=%u", (unsigned)start_pct);
}

static void device_1102_brake_task(void *param)
{
    (void)param;
    uint32_t start_pct = 0U;

    ESP_LOGI(tag_s, "brake task running");
    for (;;) {
        if (xQueueReceive(s_brake_q, &start_pct, portMAX_DELAY) != pdTRUE) {
            continue;
        }
        s_abort = false;
        device_1102_brake_run(start_pct);
    }
}

esp_err_t device_1102_brake_init(void)
{
    if (s_brake_q != NULL) {
        return ESP_OK;
    }

    s_brake_q = xQueueCreate(BRAKE_QUEUE_LEN, sizeof(uint32_t));
    if (s_brake_q == NULL) {
        ESP_LOGE(tag_s, "queue create fail");
        return ESP_ERR_NO_MEM;
    }

    esp_err_t err = app_task_start(APP_TASK_DEV1102_BRAKE, device_1102_brake_task, NULL, &s_brake_task);
    if (err != ESP_OK) {
        vQueueDelete(s_brake_q);
        s_brake_q = NULL;
        ESP_LOGE(tag_s, "task start fail err=%d", (int)err);
        return err;
    }

    ESP_LOGI(tag_s, "brake task ready name=1102_brk handle=%p", (void *)s_brake_task);
    return ESP_OK;
}

void device_1102_brake_request(uint32_t start_speed_pct)
{
    if (start_speed_pct > 100U) {
        start_speed_pct = 100U;
    }

    dev1102_brake_profile_t prof;
    dev1102_brake_profile(start_speed_pct, &prof);
    const uint32_t kick_duty = dev1102_brake_kick_duty(start_speed_pct, &prof);
    device_1102_brake_both(kick_duty);

    if (s_brake_q == NULL) {
        ESP_LOGW(tag_s, "brake request without task kick=%u%%", (unsigned)kick_duty);
        return;
    }

    s_abort = true;
    (void)xQueueOverwrite(s_brake_q, &start_speed_pct);
}

void device_1102_brake_abort(void)
{
    s_abort = true;
    device_1102_brake_both(0U);
}

bool device_1102_brake_task_ready(void)
{
    return s_brake_task != NULL;
}
