#include "common.h"
#include "car0107_control.h"
#include "modules_common.h"
#include "gpio_common.h"
#include "mqtt_infor_handle.h"

#define CTRL_NONE     0
#define CTRL_PHONE    1
#define CTRL_STEERING 2

/* 双向车模电调 RC PWM：50Hz，脉宽 1000~2000us，中位 1500us */
#define CAR0107_ESC_US_MIN          1000
#define CAR0107_ESC_US_MAX          2000
#define CAR0107_ESC_US_NEUTRAL      1500
#define CAR0107_ESC_DEADZONE_LO     1450
#define CAR0107_ESC_DEADZONE_HI     1550
#define CAR0107_ESC_RAMP_STEP_RUN      4    /* 前进线性爬升：约 4us/tick */
#define CAR0107_ESC_RAMP_STEP_BACK     2    /* 后退更慢，不得快于前进 */
#define CAR0107_ESC_BRAKE_HOLD_MS       100  /* 1200us 刹车目标保持50ms */
#define CAR0107_ESC_BRAKE_MIN_TICKS     2
#define CAR0107_ESC_REVERSE_BRAKE_MS    50  /* 前进切后退：短刹车目标50ms */
#define CAR0107_ESC_REVERSE_BRAKE_TICKS 2
#define CAR0107_ESC_NEUTRAL_UNLOCK_MS   40  /* 刹车后保持两个50Hz中位周期解锁 */
#define CAR0107_FWD_BRAKE_US           1100 /* RC无刷电调前进刹车信号 */
#define CAR0107_PHONE_VAL_MAX          200  /* App 前后油门最大值 */
#define CAR0107_FWD_US_RUN_MIN         1560 /* val=53 前进最低行走 ~1580us */
#define CAR0107_FWD_US_RUN_MAX         1700      /* 前进最大速度再降20%：val=200 -> 1700us */
#define CAR0107_BACK_US_RUN_MIN        1440  /* val=53 起步即动（须低于死区 1450） */
#define CAR0107_BACK_US_RUN_MAX        1300  /* 后退最大速度再降20%：val=200 -> 1300us */

typedef enum {
    CAR0107_ESC_DIR_FORWARD = 1, /* App mode1 前进：高脉宽（与 0101 相反） */
    CAR0107_ESC_DIR_BACK    = 2, /* App mode2 后退：低脉宽（与 0101 相反） */
} car0107_esc_dir_t;

typedef enum {
    CAR0107_ESC_NEUTRAL = 0,
    CAR0107_ESC_FORWARD,
    CAR0107_ESC_REVERSE,
    CAR0107_ESC_BRAKE_THEN_NEUTRAL,
    CAR0107_ESC_BRAKE_THEN_REVERSE,
    CAR0107_ESC_NEUTRAL_UNLOCK,
    CAR0107_ESC_TRANSITION
} car0107_esc_state_t;

typedef enum {
    CAR0107_TRANS_BRAKE = 0,       /* mode1 val<52：1200us 约50ms后回中 */
    CAR0107_TRANS_RAMP_RUN         /* 回中后正常线性爬升 */
} car0107_trans_phase_t;

typedef struct {
    car0107_esc_state_t state;
    car0107_trans_phase_t trans_phase;
    int current_us;
    int target_us;
    int cmd_dir;
    int pending_dir;
    int pending_gval;
    int brake_dir;
    long long brake_until_ms;
    int brake_hold_ticks;
    long long neutral_unlock_until_ms;
    int last_drive_dir;
    int last_nonzero_mode;
    long long pending_stop_start_ms;
    long long last_input_ms;
} car0107_esc_ctx_t;

#define STEER_STEP_TURN    60
#define STEER_STEP_RETURN  90
#define REV_TARGET_SCALE_NUM 6
#define REV_TARGET_SCALE_DEN 10
#define CAR0107_MAX_INPUT_SPEED  200
#define CAR0107_ACTIVE_THRESHOLD 50
#define CAR0107_MAX_TARGET_BACK  100
#define CAR0107_MAX_TARGET_FRONT 80
#define CAR0107_ESC_RELEASE_MAX      52   /* val<=52：松手/刹车/停下 */
#define CAR0107_ESC_GVAL_MIN         53   /* val>=53 才视为有效油门 */
#define STEER_GVAL_MAX             150

static pthread_mutex_t s_car0107_hw_mutex = PTHREAD_MUTEX_INITIALIZER;
static long long s_last_control_ms = 0;

static int s_control_source = CTRL_NONE;
static int s_target_angle = 90;
static int s_current_angle = 90;
static int s_steering_mode = 0;
static int s_steering_timeout = 0;
static int s_steering_cmd_timeout = 0;

static int s_target_speed = 0;
static int s_current_speed = 0;
static int s_speed_mode = 0;
static int s_speed_timeout = 0;

/* App 每帧分开发 mode1/mode2 两条 MQTT，必须合并后再决策，避免后退时 mode1 串扰成前进 */
static int s_phone_mode1_val = 0;
static int s_phone_mode2_val = 0;
static int s_phone_last_mode = 0;  /* 最近更新的通道，双通道同时有油时以它为准 */

static car0107_esc_ctx_t s_esc = {
    .state = CAR0107_ESC_NEUTRAL,
    .trans_phase = CAR0107_TRANS_BRAKE,
    .current_us = CAR0107_ESC_US_NEUTRAL,
    .target_us = CAR0107_ESC_US_NEUTRAL,
    .cmd_dir = 0,
    .pending_dir = 0,
    .pending_gval = 0,
    .brake_dir = 0,
    .brake_until_ms = 0,
    .brake_hold_ticks = 0,
    .neutral_unlock_until_ms = 0,
    .last_drive_dir = 0,
    .last_nonzero_mode = 0,
    .pending_stop_start_ms = 0,
    .last_input_ms = 0,
};

/* 专用电调线程：本文件 pthread_create，非全局线程池；仅 0107 设备使用 */
#define CAR0107_ESC_THREAD_MS 20
#define CAR0107_ESC_BRAKE_THREAD_MS 10

static pthread_t s_esc_thread;
static int s_esc_thread_started = 0;
static volatile int s_esc_thread_run = 0;
static pthread_mutex_t s_esc_start_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t s_esc_wake_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t s_esc_wake_cond = PTHREAD_COND_INITIALIZER;

static int car0107_clamp(int val, int lo, int hi)
{
    if (val < lo) return lo;
    if (val > hi) return hi;
    return val;
}

static int car0107_clamp_input(int val)
{
    return car0107_clamp(val, 0, CAR0107_PHONE_VAL_MAX);
}

static int car0107_input_to_gval(int val)
{
    val = car0107_clamp_input(val);
    if (val < CAR0107_ACTIVE_THRESHOLD) {
        return 0;
    }
    return 51 + (val - CAR0107_ACTIVE_THRESHOLD) *
                     (STEER_GVAL_MAX - 51) /
                     (CAR0107_MAX_INPUT_SPEED - CAR0107_ACTIVE_THRESHOLD);
}

static int car0107_input_to_target_speed(int val, int max_target)
{
    val = car0107_clamp_input(val);
    if (val < CAR0107_ACTIVE_THRESHOLD) {
        return 0;
    }
    return (val - CAR0107_ACTIVE_THRESHOLD) * max_target /
           (CAR0107_MAX_INPUT_SPEED - CAR0107_ACTIVE_THRESHOLD);
}

static int car0107_speed_to_gval(int speed)
{
    if (speed <= 0) {
        return 0;
    }
    int gval = 50 + (speed * 100) / 160;
    return car0107_clamp(gval, 0, STEER_GVAL_MAX);
}

/* App 前后油门：53~200 线性映射脉宽；val=53 即有最低行走速度，200 封顶 */
static int car0107_phone_val_to_us(int dir, int raw_val)
{
    int span;
    int t;

    if (raw_val < CAR0107_ESC_GVAL_MIN) {
        return CAR0107_ESC_US_NEUTRAL;
    }
    raw_val = car0107_clamp(raw_val, CAR0107_ESC_GVAL_MIN, CAR0107_PHONE_VAL_MAX);
    span = CAR0107_PHONE_VAL_MAX - CAR0107_ESC_GVAL_MIN;
    if (span <= 0) {
        return CAR0107_ESC_US_NEUTRAL;
    }
    t = raw_val - CAR0107_ESC_GVAL_MIN;

    if (dir == CAR0107_ESC_DIR_FORWARD) {
        return CAR0107_FWD_US_RUN_MIN +
               t * (CAR0107_FWD_US_RUN_MAX - CAR0107_FWD_US_RUN_MIN) / span;
    }
    if (dir == CAR0107_ESC_DIR_BACK) {
        return CAR0107_BACK_US_RUN_MIN -
               t * (CAR0107_BACK_US_RUN_MIN - CAR0107_BACK_US_RUN_MAX) / span;
    }
    return CAR0107_ESC_US_NEUTRAL;
}

/* 方向盘内部 gval（约 53~150） */
static int car0107_steer_gval_to_us(int dir, int gval)
{
    int span;
    int t;

    if (gval < CAR0107_ESC_GVAL_MIN) {
        return CAR0107_ESC_US_NEUTRAL;
    }
    gval = car0107_clamp(gval, CAR0107_ESC_GVAL_MIN, STEER_GVAL_MAX);
    span = STEER_GVAL_MAX - CAR0107_ESC_GVAL_MIN;
    if (span <= 0) {
        return CAR0107_ESC_US_NEUTRAL;
    }
    t = gval - CAR0107_ESC_GVAL_MIN;

    if (dir == CAR0107_ESC_DIR_FORWARD) {
        return CAR0107_FWD_US_RUN_MIN +
               t * (CAR0107_FWD_US_RUN_MAX - CAR0107_FWD_US_RUN_MIN) / span;
    }
    if (dir == CAR0107_ESC_DIR_BACK) {
        return CAR0107_BACK_US_RUN_MIN -
               t * (CAR0107_BACK_US_RUN_MIN - CAR0107_BACK_US_RUN_MAX) / span;
    }
    return CAR0107_ESC_US_NEUTRAL;
}

static int car0107_throttle_to_us(int dir, int val)
{
    if (s_control_source == CTRL_PHONE) {
        return car0107_phone_val_to_us(dir, val);
    }
    return car0107_steer_gval_to_us(dir, val);
}

static int car0107_phone_clamp_val(int raw_val)
{
    if (raw_val <= CAR0107_ESC_RELEASE_MAX) {
        return 0;
    }
    return car0107_clamp(raw_val, CAR0107_ESC_GVAL_MIN, CAR0107_PHONE_VAL_MAX);
}

static int car0107_phone_is_active(int raw_val)
{
    return raw_val > CAR0107_ESC_RELEASE_MAX;
}

static int car0107_mode1_is_release(int raw_val)
{
    return raw_val <= CAR0107_ESC_RELEASE_MAX;
}

static int car0107_esc_in_forward_context(void)
{
    if (s_esc.state == CAR0107_ESC_FORWARD) {
        return 1;
    }
    if (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) {
        return 1;
    }
    if (s_esc.current_us > CAR0107_ESC_DEADZONE_HI) {
        return 1;
    }
    return 0;
}

/*
 * 前进松手 mode1 val<52：1200us 目标保持50ms，再回中。
 * 前进中有后退意图：同样先刹车，再按 mode2 慢爬。
 */
static int car0107_forward_brake_us(void)
{
    return CAR0107_FWD_BRAKE_US;
}

static int car0107_forward_brake_pwm(void)
{
    return car0107_forward_brake_us() / 20;
}

static void car0107_esc_begin_neutral_stop(void);

static void car0107_esc_clear_pending_stop(void)
{
    s_esc.brake_dir = 0;
    s_esc.brake_until_ms = 0;
    s_esc.brake_hold_ticks = 0;
    s_esc.neutral_unlock_until_ms = 0;
    s_esc.pending_stop_start_ms = 0;
    s_esc.pending_dir = 0;
    s_esc.pending_gval = 0;
}

static void car0107_esc_write_pwm(int pwm_val)
{
    pwm_val = car0107_clamp(pwm_val, CAR0107_ESC_US_MIN / 20, CAR0107_ESC_US_MAX / 20);
    pwmWrite(PWM_PIN_SPEED, pwm_val);
    s_esc.current_us = pwm_val * 20;
}

static void car0107_esc_begin_forward_brake(void)
{
    long long now = get_current_time_millis();

    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL) {
        car0107_esc_write_pwm(car0107_forward_brake_pwm());
        return;
    }

    car0107_esc_clear_pending_stop();
    s_esc.state = CAR0107_ESC_BRAKE_THEN_NEUTRAL;
    s_esc.trans_phase = CAR0107_TRANS_BRAKE;
    s_esc.pending_stop_start_ms = now;
    s_esc.brake_dir = CAR0107_ESC_DIR_BACK;
    s_esc.brake_until_ms = now + CAR0107_ESC_BRAKE_HOLD_MS;
    s_esc.brake_hold_ticks = CAR0107_ESC_BRAKE_MIN_TICKS;
    s_esc.target_us = car0107_forward_brake_us();
    s_esc.current_us = s_esc.target_us;
    car0107_esc_write_pwm(car0107_forward_brake_pwm());
}

static void car0107_esc_begin_brake_then_reverse(int reverse_val,
                                                 int reuse_current_brake)
{
    long long now = get_current_time_millis();
    long long brake_start_ms = now;
    int elapsed_brake_ticks = 0;
    int remaining_brake_ticks = CAR0107_ESC_REVERSE_BRAKE_TICKS;

    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
        s_esc.pending_gval = reverse_val;
        return; /* 连续后退帧只更新目标，不重置短刹车计时 */
    }

    if (reuse_current_brake && s_esc.pending_stop_start_ms > 0) {
        brake_start_ms = s_esc.pending_stop_start_ms;
        elapsed_brake_ticks = CAR0107_ESC_BRAKE_MIN_TICKS -
                              s_esc.brake_hold_ticks;
        if (elapsed_brake_ticks < 0) {
            elapsed_brake_ticks = 0;
        }
        remaining_brake_ticks = CAR0107_ESC_REVERSE_BRAKE_TICKS -
                                elapsed_brake_ticks;
        if (remaining_brake_ticks < 0) {
            remaining_brake_ticks = 0;
        }
    }

    car0107_esc_clear_pending_stop();
    s_esc.state = CAR0107_ESC_BRAKE_THEN_REVERSE;
    s_esc.trans_phase = CAR0107_TRANS_BRAKE;
    s_esc.pending_dir = CAR0107_ESC_DIR_BACK;
    s_esc.pending_gval = reverse_val;
    s_esc.brake_dir = CAR0107_ESC_DIR_BACK;
    s_esc.brake_until_ms = brake_start_ms + CAR0107_ESC_REVERSE_BRAKE_MS;
    s_esc.brake_hold_ticks = remaining_brake_ticks;
    s_esc.target_us = car0107_forward_brake_us();
    car0107_esc_write_pwm(car0107_forward_brake_pwm());

    if (reuse_current_brake) {
        my_zlog_info("car0107 ESC reuse current brake: elapsed=%lldms ticks=%d remaining_ticks=%d",
                     now - brake_start_ms, elapsed_brake_ticks,
                     remaining_brake_ticks);
    }
}

static void car0107_esc_cancel_forward_brake(int fwd_val)
{
    car0107_esc_clear_pending_stop();

    if (fwd_val >= CAR0107_ESC_GVAL_MIN) {
        s_esc.cmd_dir = CAR0107_ESC_DIR_FORWARD;
        s_esc.target_us = car0107_throttle_to_us(CAR0107_ESC_DIR_FORWARD, fwd_val);
        s_esc.state = CAR0107_ESC_TRANSITION;
        s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
        return;
    }

    car0107_esc_begin_neutral_stop();
}

static void car0107_esc_begin_neutral_stop(void)
{
    s_esc.cmd_dir = 0;
    car0107_esc_clear_pending_stop();
    s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
    s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
    s_esc.state = CAR0107_ESC_NEUTRAL;
}

static void car0107_esc_begin_neutral_unlock(void)
{
    car0107_esc_clear_pending_stop();
    s_esc.cmd_dir = 0;
    s_esc.state = CAR0107_ESC_NEUTRAL_UNLOCK;
    s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
    s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
    s_esc.neutral_unlock_until_ms = get_current_time_millis() +
                                    CAR0107_ESC_NEUTRAL_UNLOCK_MS;
    car0107_esc_write_pwm(CAR0107_ESC_US_NEUTRAL / 20);
    my_zlog_info("car0107 ESC BRAKE_THEN_NEUTRAL -> NEUTRAL_UNLOCK (%dms, keep 1500us)",
                 CAR0107_ESC_NEUTRAL_UNLOCK_MS);
}

static int car0107_esc_forward_motion_active(void)
{
    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
        s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
        return 1;
    }
    if (s_esc.state == CAR0107_ESC_FORWARD) {
        return 1;
    }
    if (s_esc.cmd_dir != CAR0107_ESC_DIR_FORWARD) {
        return 0;
    }
    if (s_esc.state == CAR0107_ESC_TRANSITION &&
        s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
        return 1;
    }
    if (s_esc.state == CAR0107_ESC_TRANSITION &&
        s_esc.trans_phase == CAR0107_TRANS_BRAKE &&
        s_esc.brake_dir == CAR0107_ESC_DIR_BACK) {
        return 1;
    }
    return 0;
}

static int car0107_esc_should_forward_brake(void)
{
    if (car0107_esc_forward_motion_active()) {
        return 1;
    }
    if (s_esc.current_us > CAR0107_ESC_DEADZONE_HI) {
        return 1;
    }
    return 0;
}

static int car0107_esc_back_motion_active(void)
{
    if (s_esc.cmd_dir != CAR0107_ESC_DIR_BACK) {
        return 0;
    }
    if (s_esc.state == CAR0107_ESC_REVERSE) {
        return 1;
    }
    if (s_esc.state == CAR0107_ESC_TRANSITION &&
        s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
        return 1;
    }
    return 0;
}

static void car0107_notify_control_locked(void)
{
    s_last_control_ms = get_current_time_millis();
}

void car0107_notify_control_activity(void)
{
    pthread_mutex_lock(&s_car0107_hw_mutex);
    car0107_notify_control_locked();
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

long long car0107_last_control_ms(void)
{
    long long ts;

    pthread_mutex_lock(&s_car0107_hw_mutex);
    ts = s_last_control_ms;
    pthread_mutex_unlock(&s_car0107_hw_mutex);
    return ts;
}

int car0107_esc_is_active(void)
{
    int active;

    pthread_mutex_lock(&s_car0107_hw_mutex);
    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
        s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE ||
        s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
        active = 1;
    } else if (s_esc.state == CAR0107_ESC_NEUTRAL) {
        active = 0;
    } else {
        active = (s_esc.current_us < CAR0107_ESC_DEADZONE_LO ||
                  s_esc.current_us > CAR0107_ESC_DEADZONE_HI);
    }
    pthread_mutex_unlock(&s_car0107_hw_mutex);
    return active;
}

static void car0107_esc_output_us(int us, int snap_deadzone)
{
    int pwm_val;

    us = car0107_clamp(us, CAR0107_ESC_US_MIN, CAR0107_ESC_US_MAX);
    if (snap_deadzone &&
        us >= CAR0107_ESC_DEADZONE_LO && us <= CAR0107_ESC_DEADZONE_HI) {
        us = CAR0107_ESC_US_NEUTRAL;
    }

    pwm_val = us / 20;
    car0107_esc_write_pwm(pwm_val);
}

static void car0107_esc_write_us(int us)
{
    car0107_esc_output_us(us, 1);
}

static void car0107_esc_start_ramp(int dir, int throttle_val)
{
    int from_stop = (s_esc.state == CAR0107_ESC_NEUTRAL);

    s_esc.cmd_dir = dir;
    car0107_esc_clear_pending_stop();
    s_esc.last_drive_dir = dir;
    s_esc.last_nonzero_mode = dir;
    s_esc.target_us = car0107_throttle_to_us(dir, throttle_val);
    s_esc.state = CAR0107_ESC_TRANSITION;
    s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
    if (from_stop || dir == CAR0107_ESC_DIR_BACK) {
        s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
    }
    /* 从中位起步：val>=53 立刻跳到映射脉宽，后续只跟目标线性 */
    if (from_stop && (dir == CAR0107_ESC_DIR_FORWARD || dir == CAR0107_ESC_DIR_BACK)) {
        s_esc.current_us = s_esc.target_us;
    }
}

static void car0107_esc_phone_direct_pwm(int dir, int throttle_val)
{
    car0107_esc_state_t old_state = s_esc.state;
    int old_dir = s_esc.last_drive_dir;

    if (old_state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
        old_state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
        my_zlog_info("car0107 ESC cancel old brake/neutral timer");
    }
    car0107_esc_clear_pending_stop();
    s_esc.cmd_dir = dir;
    s_esc.last_drive_dir = dir;
    s_esc.last_nonzero_mode = dir;
    s_esc.target_us = car0107_throttle_to_us(dir, throttle_val);
    s_esc.current_us = s_esc.target_us;
    s_esc.state = (dir == CAR0107_ESC_DIR_FORWARD) ?
                  CAR0107_ESC_FORWARD : CAR0107_ESC_REVERSE;
    s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
    car0107_esc_output_us(s_esc.current_us, 0);

    if (dir == CAR0107_ESC_DIR_FORWARD) {
        if (old_dir == CAR0107_ESC_DIR_BACK && old_state != CAR0107_ESC_NEUTRAL) {
            my_zlog_info("car0107 ESC REVERSE -> FORWARD DIRECT PWM (%dus)",
                         s_esc.current_us);
        } else if (old_state == CAR0107_ESC_NEUTRAL) {
            my_zlog_info("car0107 ESC NEUTRAL -> FORWARD DIRECT PWM (%dus)",
                         s_esc.current_us);
        }
    } else if (old_state == CAR0107_ESC_NEUTRAL) {
        my_zlog_info("car0107 ESC NEUTRAL -> REVERSE_DIRECT_PWM (%dus)",
                     s_esc.current_us);
    }
}

static int car0107_esc_ramp_step_for_dir(int dir)
{
    if (dir == CAR0107_ESC_DIR_BACK) {
        return CAR0107_ESC_RAMP_STEP_BACK;
    }
    return CAR0107_ESC_RAMP_STEP_RUN;
}

static void car0107_esc_request_stop(void)
{
    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
        s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
        return;
    }

    if (car0107_esc_should_forward_brake()) {
        car0107_esc_begin_forward_brake();
        return;
    }

    if (car0107_esc_back_motion_active()) {
        car0107_esc_begin_neutral_stop();
        return;
    }

    car0107_esc_begin_neutral_stop();
}

static void car0107_esc_request(int dir, int gval)
{
    if (dir == 0 || gval < CAR0107_ESC_GVAL_MIN) {
        car0107_esc_request_stop();
        return;
    }

    if (s_esc.state == CAR0107_ESC_NEUTRAL) {
        car0107_esc_start_ramp(dir, gval);
        return;
    }

    if (s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
        s_esc.pending_dir = dir;
        s_esc.pending_gval = gval;
        return;
    }

    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
        s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
        if (dir == CAR0107_ESC_DIR_FORWARD) {
            car0107_esc_cancel_forward_brake(gval);
            s_esc.last_drive_dir = dir;
        } else {
            car0107_esc_start_ramp(dir, gval);
        }
        return;
    }

    if (s_esc.state == CAR0107_ESC_TRANSITION) {
        if (s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
            if (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD &&
                dir == CAR0107_ESC_DIR_BACK) {
                car0107_esc_start_ramp(dir, gval);
                return;
            }
            if (s_esc.cmd_dir == CAR0107_ESC_DIR_BACK &&
                dir == CAR0107_ESC_DIR_FORWARD) {
                car0107_esc_start_ramp(dir, gval);
                return;
            }
            s_esc.target_us = car0107_throttle_to_us(s_esc.cmd_dir, gval);
            return;
        }
        return;
    }

    /* 后退切前进：不刹车，直接爬升 */
    if (s_esc.cmd_dir == CAR0107_ESC_DIR_BACK && dir == CAR0107_ESC_DIR_FORWARD) {
        car0107_esc_start_ramp(dir, gval);
        return;
    }

    /* 前进切后退：不刹车，从当前脉宽慢速后退 */
    if (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD && dir == CAR0107_ESC_DIR_BACK) {
        car0107_esc_start_ramp(dir, gval);
        return;
    }

    s_esc.cmd_dir = dir;
    s_esc.pending_dir = 0;
    s_esc.pending_gval = 0;
    s_esc.target_us = car0107_throttle_to_us(dir, gval);
    s_esc.state = (dir == CAR0107_ESC_DIR_FORWARD) ? CAR0107_ESC_FORWARD : CAR0107_ESC_REVERSE;
}

static int car0107_esc_ramp_step_for(int current, int target, int dir)
{
    (void)current;
    (void)target;
    return car0107_esc_ramp_step_for_dir(dir);
}

static void car0107_phone_apply_merged(void)
{
    int mode1_raw = s_phone_mode1_val;
    int mode2_raw = s_phone_mode2_val;
    int fwd_active = car0107_phone_is_active(mode1_raw);
    int back_active = car0107_phone_is_active(mode2_raw);
    int fwd_val = car0107_phone_clamp_val(mode1_raw);
    int back_val = car0107_phone_clamp_val(mode2_raw);
    int wanted_dir = 0;
    int wanted_val = 0;

    if (fwd_active && back_active) {
        wanted_dir = (s_phone_last_mode == 2) ?
                     CAR0107_ESC_DIR_BACK : CAR0107_ESC_DIR_FORWARD;
        wanted_val = (wanted_dir == CAR0107_ESC_DIR_BACK) ? back_val : fwd_val;
    } else if (fwd_active && s_phone_last_mode == 1) {
        wanted_dir = CAR0107_ESC_DIR_FORWARD;
        wanted_val = fwd_val;
    } else if (back_active && s_phone_last_mode == 2) {
        wanted_dir = CAR0107_ESC_DIR_BACK;
        wanted_val = back_val;
    }

    if (wanted_dir == 0) {
        if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
            s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
            return;
        }
        if (s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
            my_zlog_info("car0107 ESC cancel old brake/neutral timer (reverse released)");
            car0107_esc_begin_neutral_stop();
            s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
            car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
            return;
        }
        if (s_esc.state == CAR0107_ESC_NEUTRAL) {
            return;
        }
        if (s_esc.state == CAR0107_ESC_FORWARD ||
            (s_esc.state == CAR0107_ESC_TRANSITION &&
             s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN &&
             s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) ||
            s_esc.current_us > CAR0107_ESC_DEADZONE_HI) {
            my_zlog_info("car0107 ESC FORWARD_LOW_VAL_DO_NOT_DRIVE_FORWARD: BRAKE %dus NOW",
                         car0107_forward_brake_us());
            car0107_esc_begin_forward_brake();
        } else {
            /* 后退或非前进状态松手：不进入换向判断，直接回中。 */
            if (s_esc.state == CAR0107_ESC_REVERSE) {
                my_zlog_info("car0107 ESC REVERSE -> NEUTRAL_DIRECT");
            }
            car0107_esc_begin_neutral_stop();
            s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
            car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
        }
        return;
    }

    s_esc.last_input_ms = get_current_time_millis();
    s_esc.last_nonzero_mode = wanted_dir;

    if (s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
        s_esc.pending_dir = wanted_dir;
        s_esc.pending_gval = wanted_val;
        my_zlog_info("car0107 ESC NEUTRAL_UNLOCK queue %s until minimum neutral hold completes",
                     wanted_dir == CAR0107_ESC_DIR_FORWARD ? "FORWARD" : "REVERSE");
        return;
    }

    if (wanted_dir == CAR0107_ESC_DIR_FORWARD) {
        /* 后退/停止/刹车中切前进，都取消旧定时器并直接前进。 */
        car0107_esc_phone_direct_pwm(wanted_dir, wanted_val);
        return;
    }

    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
        s_esc.pending_gval = wanted_val;
        return;
    }

    if (s_esc.state == CAR0107_ESC_FORWARD ||
        s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
        (s_esc.state == CAR0107_ESC_TRANSITION &&
         s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD)) {
        int reuse_current_brake =
            (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL);

        if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL) {
            my_zlog_info("car0107 ESC BRAKE_THEN_NEUTRAL -> BRAKE_THEN_REVERSE (reuse brake)");
        } else {
            my_zlog_info("car0107 ESC FORWARD -> BRAKE_THEN_REVERSE");
        }
        my_zlog_info("car0107 ESC cancel old brake/neutral timer");
        car0107_esc_begin_brake_then_reverse(wanted_val,
                                             reuse_current_brake);
        return;
    }

    /* NEUTRAL->REVERSE、REVERSE继续后退：直接改 PWM，不刹车。 */
    car0107_esc_phone_direct_pwm(CAR0107_ESC_DIR_BACK, wanted_val);
}

static int car0107_esc_ramp_toward(int current, int target, int step)
{
    if (current < target) {
        current += step;
        if (current > target) {
            current = target;
        }
    } else if (current > target) {
        current -= step;
        if (current < target) {
            current = target;
        }
    }
    return current;
}

static void car0107_esc_tick(void)
{
    switch (s_esc.state) {
    case CAR0107_ESC_NEUTRAL:
        /* NEUTRAL 是持续输出状态，不是停止 PWM。 */
        s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
        s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
        break;

    case CAR0107_ESC_FORWARD:
    case CAR0107_ESC_REVERSE:
        s_esc.current_us = car0107_esc_ramp_toward(
            s_esc.current_us,
            s_esc.target_us,
            car0107_esc_ramp_step_for(s_esc.current_us, s_esc.target_us,
                                      s_esc.cmd_dir));
        break;

    case CAR0107_ESC_BRAKE_THEN_NEUTRAL:
        car0107_esc_write_pwm(car0107_forward_brake_pwm());
        if (s_esc.brake_hold_ticks > 0) {
            s_esc.brake_hold_ticks--;
        }
        if (s_esc.brake_hold_ticks <= 0 &&
            get_current_time_millis() >= s_esc.brake_until_ms) {
            car0107_esc_begin_neutral_unlock();
        }
        break;

    case CAR0107_ESC_BRAKE_THEN_REVERSE:
        if (s_esc.brake_dir == CAR0107_ESC_DIR_BACK) {
            car0107_esc_write_pwm(car0107_forward_brake_pwm());
            if (s_esc.brake_hold_ticks > 0) {
                s_esc.brake_hold_ticks--;
            }
            if (s_esc.brake_hold_ticks <= 0 &&
                get_current_time_millis() >= s_esc.brake_until_ms) {
                s_esc.brake_dir = 0;
                s_esc.brake_until_ms = get_current_time_millis() +
                                       CAR0107_ESC_NEUTRAL_UNLOCK_MS;
                s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
                s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
                car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
                my_zlog_info("car0107 ESC BRAKE_THEN_REVERSE -> NEUTRAL_UNLOCK (%dms)",
                             CAR0107_ESC_NEUTRAL_UNLOCK_MS);
            }
        } else {
            int reverse_val = s_esc.pending_gval;

            car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
            if (get_current_time_millis() >= s_esc.brake_until_ms) {
                car0107_esc_clear_pending_stop();
                s_esc.cmd_dir = CAR0107_ESC_DIR_BACK;
                s_esc.last_drive_dir = CAR0107_ESC_DIR_BACK;
                s_esc.last_nonzero_mode = CAR0107_ESC_DIR_BACK;
                s_esc.target_us = car0107_throttle_to_us(CAR0107_ESC_DIR_BACK,
                                                          reverse_val);
                /* 起步点直接跳到刚过死区(1440us)，让 val>=53 立即越过死区开始后退；
                 * 再按 2us/tick 慢爬到 target，避免大 val 突跳到全速冲击电调/齿轮 */
                s_esc.current_us = (s_esc.target_us >= CAR0107_BACK_US_RUN_MIN) ?
                                    CAR0107_BACK_US_RUN_MIN : s_esc.target_us;
                s_esc.state = CAR0107_ESC_TRANSITION;
                s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
                car0107_esc_output_us(s_esc.current_us, 0);
                my_zlog_info("car0107 ESC BRAKE_THEN_REVERSE -> RAMP_REVERSE (target %dus, start %dus past deadzone)",
                             s_esc.target_us, s_esc.current_us);
            }
        }
        break;

    case CAR0107_ESC_NEUTRAL_UNLOCK:
        car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
        if (get_current_time_millis() >= s_esc.neutral_unlock_until_ms) {
            int pending_dir = s_esc.pending_dir;
            int pending_gval = s_esc.pending_gval;

            car0107_esc_clear_pending_stop();
            s_esc.state = CAR0107_ESC_NEUTRAL;
            s_esc.cmd_dir = 0;
            s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
            s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
            my_zlog_info("car0107 ESC NEUTRAL_UNLOCK -> NEUTRAL (keep 1500us continuously)");

            if (pending_dir != 0 && pending_gval >= CAR0107_ESC_GVAL_MIN) {
                if (s_control_source == CTRL_PHONE) {
                    car0107_esc_phone_direct_pwm(pending_dir, pending_gval);
                } else {
                    car0107_esc_start_ramp(pending_dir, pending_gval);
                }
            }
        }
        break;

    case CAR0107_ESC_TRANSITION:
        s_esc.current_us = car0107_esc_ramp_toward(s_esc.current_us,
                                                   s_esc.target_us,
                                                   car0107_esc_ramp_step_for_dir(
                                                       s_esc.cmd_dir));
        if (s_esc.current_us == s_esc.target_us) {
            s_esc.state = (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) ?
                          CAR0107_ESC_FORWARD : CAR0107_ESC_REVERSE;
        }
        break;

    default:
        break;
    }

    if (s_esc.state != CAR0107_ESC_BRAKE_THEN_NEUTRAL &&
        s_esc.state != CAR0107_ESC_BRAKE_THEN_REVERSE) {
        int snap = (s_esc.state == CAR0107_ESC_NEUTRAL);
        car0107_esc_output_us(s_esc.current_us, snap);
    }
}

static void car0107_calculate_L_R(int angle)
{
    if (angle < 0) {
        angle = 0;
    } else if (angle > 180) {
        angle = 180;
    }
    float pulse_width = (angle / 180.0) * 2 + 0.5;
    float period = 1000.0 / 50;
    int val = (int)((pulse_width / period) * 1000);
    pwmWrite(PWM_PIN_CHANGE, val);
}

void car0107_steering_set_angle_sync(int angle)
{
    pthread_mutex_lock(&s_car0107_hw_mutex);
    s_current_angle = angle;
    s_target_angle = angle;
    car0107_calculate_L_R(angle);
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

static void car0107_reset_steering_state(void)
{
    s_control_source = CTRL_NONE;
    s_target_speed = 0;
    s_current_speed = 0;
    s_speed_mode = 0;
    s_phone_mode1_val = 0;
    s_phone_mode2_val = 0;
    s_phone_last_mode = 0;
    s_target_angle = 90;
    s_current_angle = 90;
    s_steering_mode = 0;
    s_steering_cmd_timeout = 0;
    s_speed_timeout = 0;
    s_steering_timeout = 0;
}

static void car0107_reset_esc_state(void)
{
    s_esc.state = CAR0107_ESC_NEUTRAL;
    s_esc.trans_phase = CAR0107_TRANS_BRAKE;
    s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
    s_esc.cmd_dir = 0;
    s_esc.pending_dir = 0;
    s_esc.pending_gval = 0;
    s_esc.brake_dir = 0;
    s_esc.brake_until_ms = 0;
    s_esc.brake_hold_ticks = 0;
    s_esc.neutral_unlock_until_ms = 0;
    s_esc.last_drive_dir = 0;
    s_esc.last_nonzero_mode = 0;
    s_esc.pending_stop_start_ms = 0;
    s_esc.last_input_ms = 0;
}

static void car0107_apply_stop(void)
{
    car0107_reset_esc_state();
    car0107_esc_write_us(CAR0107_ESC_US_NEUTRAL);
    car0107_calculate_L_R(90);
}

static void car0107_stop_speed_now(void)
{
    s_target_speed = 0;
    s_current_speed = 0;
    s_speed_mode = 0;
    s_speed_timeout = 0;
    car0107_esc_request(0, 0);
}

void car0107_middle_pwm(void)
{
    int forward_needs_brake;

    /* 0107 初始化和异常停止都保证专用线程存在，持续刷新 1500us 中值。 */
    car0107_esc_thread_start();
    pthread_mutex_lock(&s_car0107_hw_mutex);

    forward_needs_brake =
        (s_esc.state == CAR0107_ESC_FORWARD ||
         s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
         s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE ||
         (s_esc.state == CAR0107_ESC_TRANSITION &&
          s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) ||
         s_esc.current_us > CAR0107_ESC_DEADZONE_HI);

    car0107_reset_steering_state();
    if (forward_needs_brake) {
        my_zlog_info("car0107 ESC AUTO_STOP FORWARD -> BRAKE_THEN_NEUTRAL (%dus)",
                     car0107_forward_brake_us());
        car0107_esc_begin_forward_brake();
        car0107_calculate_L_R(90);
    } else {
        /* 后退/中位异常停止不需要刹车，直接持续输出1500us。 */
        car0107_apply_stop();
    }
    /* 异常停止不算用户控制，不刷新 idle 保活计时 */
    pthread_mutex_unlock(&s_car0107_hw_mutex);
    car0107_esc_thread_wake();
}

static void car0107_mode_3_left(int gval)
{
    int b = 15;
    if (gval < 45) {
        car0107_calculate_L_R(90);
    } else if (gval < 70) {
        car0107_calculate_L_R(50 + gval + b);
    } else {
        car0107_calculate_L_R(135);
    }
}

static void car0107_mode_4_right(int gval)
{
    int b = 10;
    if (gval < 45) {
        car0107_calculate_L_R(90);
    } else if (gval < 70) {
        car0107_calculate_L_R(130 - gval - b);
    } else {
        car0107_calculate_L_R(35);
    }
}

void car0107_control_change(int *buf)
{
    int mode = buf[1];
    int raw_val = car0107_clamp_input(buf[2]);

    car0107_esc_thread_start();

    pthread_mutex_lock(&g_exit_count_mutex);
    g_devcontrol_exit_count = 0;
    pthread_mutex_unlock(&g_exit_count_mutex);

    if (mode == 1 || mode == 2) {
        pthread_mutex_lock(&s_car0107_hw_mutex);
        s_control_source = CTRL_PHONE;
        s_target_speed = 0;
        s_current_speed = 0;
        s_speed_mode = 0;
        s_speed_timeout = 0;
        s_steering_timeout = 0;
        car0107_notify_control_locked();
        s_esc.last_input_ms = get_current_time_millis();

        if (mode == 1) {
            s_phone_mode1_val = raw_val;
            if (car0107_phone_is_active(raw_val)) {
                s_phone_last_mode = 1;
            }
        } else {
            s_phone_mode2_val = raw_val;
            if (car0107_phone_is_active(raw_val)) {
                s_phone_last_mode = 2;
            }
        }

        /* 前进只认 mode1 低值停车；mode2,val=0 是前进伴随空帧，必须忽略。 */
        if (mode == 1 && raw_val < CAR0107_ESC_RELEASE_MAX &&
            car0107_esc_should_forward_brake()) {
            s_phone_mode1_val = 0;
            s_phone_mode2_val = 0;
            s_phone_last_mode = 0;
            my_zlog_info("car0107 ESC FORWARD STOP mode=1 val=%d: BRAKE %dus for %dms",
                         raw_val, car0107_forward_brake_us(),
                         CAR0107_ESC_BRAKE_HOLD_MS);
            car0107_esc_begin_forward_brake();
            pthread_mutex_unlock(&s_car0107_hw_mutex);
            car0107_esc_thread_wake();
            return;
        }

        /* 后退自己的 mode2,val=0 才表示后退停止；前进状态下同帧不会进这里。 */
        if (mode == 2 && raw_val == 0 &&
            (s_esc.state == CAR0107_ESC_REVERSE ||
             s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE)) {
            s_phone_mode1_val = 0;
            s_phone_mode2_val = 0;
            s_phone_last_mode = 0;
            my_zlog_info("car0107 ESC REVERSE STOP mode=2 val=0 -> NEUTRAL 1500us");
            car0107_esc_begin_neutral_stop();
            car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
            pthread_mutex_unlock(&s_car0107_hw_mutex);
            car0107_esc_thread_wake();
            return;
        }

        /* MQTT 边沿立刻决策；刹车阶段由电调线程独占 tick，避免同包内结束刹车 */
        car0107_phone_apply_merged();
        if (s_esc.state != CAR0107_ESC_BRAKE_THEN_NEUTRAL &&
            s_esc.state != CAR0107_ESC_BRAKE_THEN_REVERSE) {
            car0107_esc_tick();
        }
        pthread_mutex_unlock(&s_car0107_hw_mutex);
        car0107_esc_thread_wake();
        return;
    }

    pthread_mutex_lock(&s_car0107_hw_mutex);
    s_control_source = CTRL_PHONE;
    s_target_speed = 0;
    s_current_speed = 0;
    s_speed_mode = 0;
    s_speed_timeout = 0;
    s_steering_timeout = 0;
    car0107_notify_control_locked();

    switch (mode) {
    case 3:
        car0107_mode_3_left(raw_val);
        break;
    case 4:
        car0107_mode_4_right(raw_val);
        break;
    default:
        break;
    }
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

static void car0107_speed_smooth_process(void)
{
    if (s_control_source != CTRL_STEERING) {
        return;
    }

    s_speed_timeout--;
    if (s_speed_timeout <= 0) {
        s_current_speed = 0;
        car0107_esc_request(0, 0);
        if (s_steering_timeout <= 0) {
            s_control_source = CTRL_NONE;
        }
        return;
    }

    int limited_target = s_target_speed;
    if (s_speed_mode == 1) {
        limited_target = (limited_target * REV_TARGET_SCALE_NUM) / REV_TARGET_SCALE_DEN;
    }

    if (s_speed_mode == 0) {
        if (s_current_speed > 0) {
            s_current_speed -= 6;
            if (s_current_speed < 0) {
                s_current_speed = 0;
            }
        }
        car0107_esc_request(0, 0);
    } else if (s_speed_mode == 1 || s_speed_mode == 2) {
        if (s_current_speed < limited_target) {
            s_current_speed += 5;
        } else if (s_current_speed > limited_target) {
            s_current_speed -= 6;
        }

        if (s_current_speed <= 0) {
            car0107_esc_request(0, 0);
        } else {
            int gval = car0107_speed_to_gval(s_current_speed);
            int dir = (s_speed_mode == 1) ? CAR0107_ESC_DIR_BACK : CAR0107_ESC_DIR_FORWARD;
            car0107_esc_request(dir, gval);
        }
    }
}

static void car0107_steering_angle_smooth_process(void)
{
    if (s_control_source != CTRL_STEERING) {
        return;
    }

    s_steering_timeout--;
    if (s_steering_timeout <= 0) {
        if (s_speed_timeout <= 0) {
            s_control_source = CTRL_NONE;
        }
        s_steering_mode = 0;
        s_target_angle = 90;
        return;
    }

    if (s_steering_cmd_timeout > 0) {
        s_steering_cmd_timeout--;
    }
    if (s_steering_cmd_timeout <= 0) {
        s_steering_mode = 0;
        s_target_angle = 90;
    }

    int wanted_angle = 90;
    int input_angle = s_target_angle;
    int smooth_step = STEER_STEP_TURN;

    if (input_angle < 0) input_angle = 0;
    if (input_angle > 180) input_angle = 180;

    if (s_steering_mode == 3) {
        wanted_angle = 90 + (input_angle * 37) / 180;
    } else if (s_steering_mode == 4) {
        wanted_angle = 90 - (input_angle * 37) / 180;
    } else {
        wanted_angle = 90;
        smooth_step = STEER_STEP_RETURN;
    }

    if (wanted_angle > 130) wanted_angle = 130;
    if (wanted_angle < 50) wanted_angle = 50;

    if (s_current_angle < wanted_angle) {
        s_current_angle += smooth_step;
        if (s_current_angle > wanted_angle) s_current_angle = wanted_angle;
    } else if (s_current_angle > wanted_angle) {
        s_current_angle -= smooth_step;
        if (s_current_angle < wanted_angle) s_current_angle = wanted_angle;
    }

    car0107_calculate_L_R(s_current_angle);
}

void car0107_steering_smooth_all(void)
{
    pthread_mutex_lock(&s_car0107_hw_mutex);
    car0107_steering_angle_smooth_process();
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

void car0107_control_steering_change_process(int *buf)
{
    int mode = buf[1];
    int val = buf[2];

    car0107_esc_thread_start();

    pthread_mutex_lock(&s_car0107_hw_mutex);
    s_control_source = CTRL_STEERING;
    s_steering_timeout = 200;
    s_speed_timeout = 200;
    car0107_notify_control_locked();

    if (mode == 1) {
        val = car0107_clamp_input(val);
        if (val == 0 || val < CAR0107_ACTIVE_THRESHOLD) {
            car0107_stop_speed_now();
        } else {
            s_speed_mode = 1;
            s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_BACK);
        }
    } else if (mode == 2) {
        val = car0107_clamp_input(val);
        if (val == 0 || val < CAR0107_ACTIVE_THRESHOLD) {
            car0107_stop_speed_now();
        } else {
            s_speed_mode = 2;
            s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_FRONT);
        }
    } else if (mode == 3) {
        if (val == 0) {
            s_steering_mode = 0;
            s_target_angle = 90;
            s_current_angle = 90;
            s_steering_cmd_timeout = 0;
            car0107_calculate_L_R(90);
        } else {
            s_steering_cmd_timeout = 60;
            s_target_angle = val;
            s_steering_mode = 3;
            if (s_target_angle < 0) s_target_angle = 0;
            if (s_target_angle > 180) s_target_angle = 180;
        }
    } else if (mode == 4) {
        if (val == 0) {
            s_steering_mode = 0;
            s_target_angle = 90;
            s_current_angle = 90;
            s_steering_cmd_timeout = 0;
            car0107_calculate_L_R(90);
        } else {
            s_steering_cmd_timeout = 60;
            s_target_angle = val;
            s_steering_mode = 4;
            if (s_target_angle < 0) s_target_angle = 0;
            if (s_target_angle > 180) s_target_angle = 180;
        }
    }
    pthread_mutex_unlock(&s_car0107_hw_mutex);
    car0107_esc_thread_wake();
}

static void car0107_esc_thread_add_timespec_ms(struct timespec *ts, int ms)
{
    ts->tv_nsec += (long)ms * 1000000L;
    if (ts->tv_nsec >= 1000000000L) {
        ts->tv_sec += ts->tv_nsec / 1000000000L;
        ts->tv_nsec %= 1000000000L;
    }
}

static int car0107_esc_thread_process_once(void)
{
    int wait_ms = CAR0107_ESC_THREAD_MS;

    pthread_mutex_lock(&s_car0107_hw_mutex);
    if (s_control_source == CTRL_STEERING) {
        car0107_speed_smooth_process();
    }
    car0107_esc_tick();
    if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
        s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
        wait_ms = CAR0107_ESC_BRAKE_THREAD_MS;
    }
    pthread_mutex_unlock(&s_car0107_hw_mutex);
    return wait_ms;
}

static void *car0107_esc_thread_main(void *arg)
{
    (void)arg;

    while (s_esc_thread_run) {
        int wait_ms = car0107_esc_thread_process_once();

        pthread_mutex_lock(&s_esc_wake_mutex);
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);
        car0107_esc_thread_add_timespec_ms(&ts, wait_ms);
        pthread_cond_timedwait(&s_esc_wake_cond, &s_esc_wake_mutex, &ts);
        pthread_mutex_unlock(&s_esc_wake_mutex);
    }
    return NULL;
}

void car0107_esc_thread_wake(void)
{
    int started;

    pthread_mutex_lock(&s_esc_start_mutex);
    started = s_esc_thread_started;
    pthread_mutex_unlock(&s_esc_start_mutex);
    if (!started) {
        return;
    }
    pthread_mutex_lock(&s_esc_wake_mutex);
    pthread_cond_signal(&s_esc_wake_cond);
    pthread_mutex_unlock(&s_esc_wake_mutex);
}

void car0107_esc_thread_start(void)
{
    pthread_mutex_lock(&s_esc_start_mutex);
    if (s_esc_thread_started) {
        pthread_mutex_unlock(&s_esc_start_mutex);
        return;
    }
    s_esc_thread_run = 1;
    if (pthread_create(&s_esc_thread, NULL, car0107_esc_thread_main, NULL) != 0) {
        s_esc_thread_run = 0;
        pthread_mutex_unlock(&s_esc_start_mutex);
        my_zlog_error("car0107 esc thread create failed");
        return;
    }
    s_esc_thread_started = 1;
    pthread_mutex_unlock(&s_esc_start_mutex);
    my_zlog_info("car0107 esc thread started (%dms tick, dedicated pthread)",
                 CAR0107_ESC_THREAD_MS);
}

void car0107_esc_thread_close(void)
{
    pthread_t thread;

    pthread_mutex_lock(&s_esc_start_mutex);
    if (!s_esc_thread_started) {
        pthread_mutex_unlock(&s_esc_start_mutex);
        return;
    }
    s_esc_thread_run = 0;
    thread = s_esc_thread;
    pthread_mutex_unlock(&s_esc_start_mutex);

    car0107_esc_thread_wake();
    pthread_join(thread, NULL);

    pthread_mutex_lock(&s_esc_start_mutex);
    s_esc_thread_started = 0;
    pthread_mutex_unlock(&s_esc_start_mutex);
    my_zlog_info("car0107 esc thread closed");
}
