#include "device_1101.h"
#include "rc_pwm_control.h"

/*
 * 1101: throttle on IO15 (ESC); steering on IO16 (SERVO, via rc_pwm_set_steering_angle_deg).
 * No onboard driver IC (IO10/21/11/12 unused). Default menuconfig: IO15=ESC, IO16=SERVO.
 */
#define DEV1101_VAL_MAX            200
#define DEV1101_DRIVE_START_TH     50
#define DEV1101_ESC_NEUTRAL_PCT    50U   /* 1500us 中位 */

#define DEV1101_STEER_VAL_LO       45
#define DEV1101_STEER_VAL_HI       70
#define DEV1101_STEER_VAL_SPAN     (69 - DEV1101_STEER_VAL_LO)
#define DEV1101_STEER_CENTER_ANG   90
#define DEV1101_STEER_DELTA_ANG    30
#define DEV1101_STEER_START_DELTA  12
#define DEV1101_STEER_LEFT_MAX     (DEV1101_STEER_CENTER_ANG + DEV1101_STEER_DELTA_ANG)
#define DEV1101_STEER_RIGHT_MAX    (DEV1101_STEER_CENTER_ANG - DEV1101_STEER_DELTA_ANG)

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 esc_forward_percent_from_val(int val)
{
    int v = clamp_int(val, 0, DEV1101_VAL_MAX);
    if (v <= DEV1101_DRIVE_START_TH) {
        return DEV1101_ESC_NEUTRAL_PCT;
    }
    /* 50~200 → 中位~满前进（50%~100% → 1500~2000us） */
    return 50U + (uint32_t)(v - DEV1101_DRIVE_START_TH) * 50U /
                      (uint32_t)(DEV1101_VAL_MAX - DEV1101_DRIVE_START_TH);
}

static uint32_t esc_backward_percent_from_val(int val)
{
    int v = clamp_int(val, 0, DEV1101_VAL_MAX);
    if (v <= DEV1101_DRIVE_START_TH) {
        return DEV1101_ESC_NEUTRAL_PCT;
    }
    /* 50~200 → 中位~满后退（50%~0% → 1500~1000us） */
    return 50U - (uint32_t)(v - DEV1101_DRIVE_START_TH) * 50U /
                      (uint32_t)(DEV1101_VAL_MAX - DEV1101_DRIVE_START_TH);
}

static void device_1101_stop(void)
{
    rc_pwm_set_esc_neutral();
    rc_pwm_set_steering_angle_deg(DEV1101_STEER_CENTER_ANG);
}

static void device_1101_drive_forward(int speed_val)
{
    rc_pwm_set_dual_esc_percent(esc_forward_percent_from_val(speed_val));
}

static void device_1101_drive_backward(int speed_val)
{
    rc_pwm_set_dual_esc_percent(esc_backward_percent_from_val(speed_val));
}

static void device_1101_steer_left(int speed_val)
{
    if (speed_val < DEV1101_STEER_VAL_LO) {
        rc_pwm_set_steering_angle_deg(DEV1101_STEER_CENTER_ANG);
    } else if (speed_val < DEV1101_STEER_VAL_HI) {
        uint32_t ang = (uint32_t)(DEV1101_STEER_CENTER_ANG + DEV1101_STEER_START_DELTA +
                                  (uint32_t)(speed_val - DEV1101_STEER_VAL_LO) *
                                      (DEV1101_STEER_DELTA_ANG - DEV1101_STEER_START_DELTA) /
                                      (uint32_t)DEV1101_STEER_VAL_SPAN);
        rc_pwm_set_steering_angle_deg(ang);
    } else {
        rc_pwm_set_steering_angle_deg(DEV1101_STEER_LEFT_MAX);
    }
}

static void device_1101_steer_right(int speed_val)
{
    if (speed_val < DEV1101_STEER_VAL_LO) {
        rc_pwm_set_steering_angle_deg(DEV1101_STEER_CENTER_ANG);
    } else if (speed_val < DEV1101_STEER_VAL_HI) {
        uint32_t ang = (uint32_t)(DEV1101_STEER_CENTER_ANG - DEV1101_STEER_START_DELTA -
                                  (uint32_t)(speed_val - DEV1101_STEER_VAL_LO) *
                                      (DEV1101_STEER_DELTA_ANG - DEV1101_STEER_START_DELTA) /
                                      (uint32_t)DEV1101_STEER_VAL_SPAN);
        rc_pwm_set_steering_angle_deg(ang);
    } else {
        rc_pwm_set_steering_angle_deg(DEV1101_STEER_RIGHT_MAX);
    }
}

static void device_1101_control(int mode, int speed_val, int steer_val)
{
    switch (mode) {
    case 1:
        device_1101_drive_forward(speed_val);
        break;
    case 2:
        device_1101_drive_backward(speed_val);
        break;
    case 3:
        device_1101_steer_left(speed_val);
        break;
    case 4:
        device_1101_steer_right(speed_val);
        break;
    case 5:
        device_1101_drive_forward(speed_val);
        device_1101_steer_left(steer_val);
        break;
    case 6:
        device_1101_drive_forward(speed_val);
        device_1101_steer_right(steer_val);
        break;
    case 7:
        device_1101_drive_backward(speed_val);
        device_1101_steer_left(steer_val);
        break;
    case 8:
        device_1101_drive_backward(speed_val);
        device_1101_steer_right(steer_val);
        break;
    default:
        break;
    }
}

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

static const device_drive_ops_t ops_s = {
    .name = "1101",
    .stop = device_1101_stop,
    .control = device_1101_control,
    .shot = device_1101_shot,
};

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