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

static void device_1101_stop(void)
{
    rc_pwm_stop_all_drive_outputs();
}

/*
 * 当前 1101 先复用 1201 的控制映射，后续若协议或硬件差异扩大，
 * 只需在本文件内调整映射，不影响上层协议处理。
 */
static void device_1101_control(int mode, int speed_val, int steer_val)
{
    (void)steer_val;
    if (mode == 1) {
        if (speed_val < 50) {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, 0);
        } else {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, speed_val / 2 - 10);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, 0);
        }
    } else if (mode == 2) {
        if (speed_val < 50) {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, 0);
        } else {
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV1_A, 0);
            rc_pwm_set_drive_percent(RC_PWM_PIN_DRV2_A, speed_val / 2 - 10);
        }
    } else if (mode == 3) {
        if (speed_val < 45) {
            rc_pwm_set_steering_angle_deg(90);
        } else if (speed_val < 70) {
            rc_pwm_set_steering_angle_deg(50 + speed_val + 7);
        } else {
            rc_pwm_set_steering_angle_deg(135);
        }
    } else if (mode == 4) {
        if (speed_val < 45) {
            rc_pwm_set_steering_angle_deg(90);
        } else if (speed_val < 70) {
            rc_pwm_set_steering_angle_deg(130 - speed_val - 7);
        } else {
            rc_pwm_set_steering_angle_deg(45);
        }
    }
}

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;
}
