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

#define DEV1102_VAL_MAX            200
#define DEV1102_DRIVE_START_TH     50
/* 转向：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 int clamp_int(int v, int lo, int hi)
{
    if (v < lo) {
        return lo;
    }
    if (v > hi) {
        return hi;
    }
    return v;
}

static uint32_t drive_percent_from_val_200(int val)
{
    int v = clamp_int(val, 0, DEV1102_VAL_MAX);
    if (v <= DEV1102_DRIVE_START_TH) {
        return 0;
    }
    /* 50~200 线性映射到 0~100% */
    return (uint32_t)(((v - DEV1102_DRIVE_START_TH) * 100) /
                      (DEV1102_VAL_MAX - DEV1102_DRIVE_START_TH));
}

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)
{
    /* 1102: 10 前进，21 后退；全部拉低并将 IO16 回中 */
    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_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, DEV1102_STEER_CENTER_ANG);
}

static void device_1102_drive_forward(uint32_t 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)
{
    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: { /* 前进：IO10，转向回中 */
        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: { /* 后退：IO21，转向回中 */
        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: { /* 左转：IO16 舵机 */
        rc_pwm_set_aux_servo_angle_deg(RC_PWM_PIN_AUX_16, steer_left_angle_from_val(speed));
        break;
    }
    case 4: { /* 右转：IO16 舵机 */
        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)
{
    return &ops_s;
}
