#include "device_1102.h"
#include "device_1102_brake.h"
#include "rc_pwm_control.h"

#define DEV1102_VAL_MAX            200
#define DEV1102_DRIVE_STOP_TH      50
#define DEV1102_DRIVE_VAL_MIN      51
#define DEV1102_DRIVE_PWM_MIN      28U
#define DEV1102_DRIVE_PWM_MAX      100U
#define DEV1102_DRIVE_VAL_SPAN     (DEV1102_VAL_MAX - DEV1102_DRIVE_VAL_MIN)
/* 转向：App 侧 val 在 [47,62] 线性对应角度；中位 90°，左右各 ±30° */
#define DEV1102_STEER_VAL_LO       47
#define DEV1102_STEER_VAL_HI       62
#define DEV1102_STEER_CENTER_ANG   90
#define DEV1102_STEER_DELTA_ANG    30
#define DEV1102_STEER_LEFT_MAX     (DEV1102_STEER_CENTER_ANG + DEV1102_STEER_DELTA_ANG)
#define DEV1102_STEER_RIGHT_MAX    (DEV1102_STEER_CENTER_ANG - DEV1102_STEER_DELTA_ANG)
#define DEV1102_STEER_VAL_SPAN     (DEV1102_STEER_VAL_HI - DEV1102_STEER_VAL_LO)

static uint32_t s_last_drive_pct;
static bool s_brake_inited;

static int clamp_int(int v, int lo, int hi)
{
    if (v < lo) {
        return lo;
    }
    if (v > hi) {
        return hi;
    }
    return v;
}

static void device_1102_ensure_brake_task(void)
{
    if (!s_brake_inited) {
        if (device_1102_brake_init() == ESP_OK) {
            s_brake_inited = true;
        }
    }
}

static uint32_t drive_percent_from_val_200(int val)
{
    int v = clamp_int(val, 0, DEV1102_VAL_MAX);
    if (v <= DEV1102_DRIVE_STOP_TH) {
        return 0;
    }
    if (v >= DEV1102_VAL_MAX) {
        return DEV1102_DRIVE_PWM_MAX;
    }
    /* 51~200 线性映射到 28%~100% 驱动占空比 */
    return DEV1102_DRIVE_PWM_MIN +
           (uint32_t)(v - DEV1102_DRIVE_VAL_MIN) * (DEV1102_DRIVE_PWM_MAX - DEV1102_DRIVE_PWM_MIN) /
               (uint32_t)DEV1102_DRIVE_VAL_SPAN;
}

static void device_1102_request_brake(void)
{
    device_1102_ensure_brake_task();
    device_1102_brake_request(s_last_drive_pct);
}

static uint32_t steer_left_angle_from_val(int val)
{
    int v = clamp_int(val, 0, DEV1102_VAL_MAX);
    if (v <= DEV1102_STEER_VAL_LO) {
        return DEV1102_STEER_CENTER_ANG;
    }
    if (v >= DEV1102_STEER_VAL_HI) {
        return DEV1102_STEER_LEFT_MAX;
    }
    return (uint32_t)(DEV1102_STEER_CENTER_ANG +
                      (uint32_t)(v - DEV1102_STEER_VAL_LO) * DEV1102_STEER_DELTA_ANG /
                          (uint32_t)DEV1102_STEER_VAL_SPAN);
}

static uint32_t steer_right_angle_from_val(int val)
{
    int v = clamp_int(val, 0, DEV1102_VAL_MAX);
    if (v <= DEV1102_STEER_VAL_LO) {
        return DEV1102_STEER_CENTER_ANG;
    }
    if (v >= DEV1102_STEER_VAL_HI) {
        return DEV1102_STEER_RIGHT_MAX;
    }
    return (uint32_t)(DEV1102_STEER_CENTER_ANG -
                      (uint32_t)(v - DEV1102_STEER_VAL_LO) * DEV1102_STEER_DELTA_ANG /
                          (uint32_t)DEV1102_STEER_VAL_SPAN);
}

static void device_1102_stop(void)
{
    device_1102_request_brake();
    rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, DEV1102_STEER_CENTER_ANG);
}

static void device_1102_drive_forward(uint32_t pct)
{
    if (pct == 0U) {
        device_1102_request_brake();
        return;
    }
    device_1102_ensure_brake_task();
    device_1102_brake_abort();
    s_last_drive_pct = pct;
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, pct);
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_B, 0);
}

static void device_1102_drive_backward(uint32_t pct)
{
    if (pct == 0U) {
        device_1102_request_brake();
        return;
    }
    device_1102_ensure_brake_task();
    device_1102_brake_abort();
    s_last_drive_pct = pct;
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
    rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_B, pct);
}

static void device_1102_control(int mode, int speed_val, int steer_val)
{
    int speed = clamp_int(speed_val, 0, DEV1102_VAL_MAX);
    int steer = clamp_int(steer_val, 0, DEV1102_VAL_MAX);
    switch (mode) {
    case 1: {
        uint32_t pct = drive_percent_from_val_200(speed);
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, DEV1102_STEER_CENTER_ANG);
        device_1102_drive_forward(pct);
        break;
    }
    case 2: {
        uint32_t pct = drive_percent_from_val_200(speed);
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, DEV1102_STEER_CENTER_ANG);
        device_1102_drive_backward(pct);
        break;
    }
    case 3:
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, steer_left_angle_from_val(speed));
        break;
    case 4:
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, steer_right_angle_from_val(speed));
        break;
    case 5:
        device_1102_drive_forward(drive_percent_from_val_200(speed));
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, steer_left_angle_from_val(steer));
        break;
    case 6:
        device_1102_drive_forward(drive_percent_from_val_200(speed));
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, steer_right_angle_from_val(steer));
        break;
    case 7:
        device_1102_drive_backward(drive_percent_from_val_200(speed));
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, steer_left_angle_from_val(steer));
        break;
    case 8:
        device_1102_drive_backward(drive_percent_from_val_200(speed));
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, steer_right_angle_from_val(steer));
        break;
    default:
        break;
    }
}

static void device_1102_shot(int pin, int val)
{
    (void)pin;
    (void)val;
}

static const device_drive_ops_t ops_s = {
    .name = "1102",
    .stop = device_1102_stop,
    .control = device_1102_control,
    .shot = device_1102_shot,
};

const device_drive_ops_t *device_1102_get_ops(void)
{
    device_1102_ensure_brake_task();
    return &ops_s;
}
