#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_NEUTRAL  8
#define CAR0107_ESC_RAMP_STEP_RUN      4    /* 正常前进/后退线性爬升：约 4us/tick */
#define CAR0107_ESC_NEUTRAL_HOLD_TICKS 8
#define CAR0107_ESC_BRAKE_HOLD_TICKS   20   /* 约 500ms @25ms tick，满刹保持 */
#define CAR0107_PHONE_VAL_MAX          200  /* App 前后油门最大值 */
#define CAR0107_FORWARD_PWM_CAP        83   /* 前进 PWM 封顶 */
#define CAR0107_FWD_US_RUN_MIN         (79 * 20) /* val=53 前进最低行走 ~1580us */
#define CAR0107_FWD_US_RUN_MAX         (CAR0107_FORWARD_PWM_CAP * 20) /* val=200 ~1660us */
#define CAR0107_BACK_US_RUN_MIN        (71 * 20) /* val=53 后退最低行走 ~1420us */
#define CAR0107_BACK_US_RUN_MAX        (58 * 20) /* val=200 后退最深 ~1160us */

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_STOP = 0,
    CAR0107_ESC_FORWARD,
    CAR0107_ESC_REVERSE,
    CAR0107_ESC_TRANSITION
} car0107_esc_state_t;

typedef enum {
    CAR0107_TRANS_BRAKE = 0,       /* 前进松手/换后退：1000us 满刹 */
    CAR0107_TRANS_NEUTRAL_HOLD,    /* 回中位 1500us */
    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;
    int brake_ticks;
    int neutral_hold_ticks;
} 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_STOP,
    .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_ticks = 0,
    .neutral_hold_ticks = 0,
};

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

/*
 * 仅前进松手/换后退时打 1000us 刹车。
 * 正常后退最深约 1160us，与刹车 1000us 分开。
 */
static int car0107_forward_brake_us(void)
{
    return CAR0107_ESC_US_MIN;
}

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

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(int pending_dir, int pending_gval)
{
    s_esc.state = CAR0107_ESC_TRANSITION;
    s_esc.trans_phase = CAR0107_TRANS_BRAKE;
    s_esc.pending_dir = pending_dir;
    s_esc.pending_gval = pending_gval;
    s_esc.brake_dir = CAR0107_ESC_DIR_BACK;
    s_esc.brake_ticks = CAR0107_ESC_BRAKE_HOLD_TICKS;
    s_esc.neutral_hold_ticks = 0;
    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_neutral_stop(void)
{
    s_esc.cmd_dir = 0;
    s_esc.pending_dir = 0;
    s_esc.pending_gval = 0;
    s_esc.brake_dir = 0;
    s_esc.brake_ticks = 0;
    s_esc.neutral_hold_ticks = 0;
    s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
    s_esc.state = CAR0107_ESC_STOP;
}

static int car0107_esc_forward_motion_active(void)
{
    if (s_esc.cmd_dir != CAR0107_ESC_DIR_FORWARD) {
        return 0;
    }
    if (s_esc.state == CAR0107_ESC_FORWARD) {
        return 1;
    }
    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_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_TRANSITION) {
        active = (s_esc.trans_phase == CAR0107_TRANS_BRAKE);
    } else if (s_esc.state == CAR0107_ESC_STOP) {
        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_STOP);

    s_esc.cmd_dir = dir;
    s_esc.pending_dir = 0;
    s_esc.pending_gval = 0;
    s_esc.brake_dir = 0;
    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) {
        /* val>52 首帧立即到对应脉宽（53=最低行走速度） */
        s_esc.current_us = s_esc.target_us;
    }
}

static void car0107_esc_begin_transition(int pending_dir, int pending_gval)
{
    if (s_esc.state == CAR0107_ESC_TRANSITION &&
        s_esc.pending_dir == pending_dir &&
        s_esc.trans_phase != CAR0107_TRANS_BRAKE) {
        s_esc.pending_gval = pending_gval;
        return;
    }

    /* 前进中换后退：先 1000us 刹车 */
    car0107_esc_begin_forward_brake(pending_dir, pending_gval);
}

static void car0107_esc_request_stop(void)
{
    if (car0107_esc_forward_motion_active()) {
        car0107_esc_begin_forward_brake(0, 0);
        return;
    }

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

    if (s_esc.state == CAR0107_ESC_TRANSITION &&
        s_esc.trans_phase == CAR0107_TRANS_BRAKE) {
        return;
    }

    if (s_esc.state == CAR0107_ESC_TRANSITION &&
        s_esc.trans_phase == CAR0107_TRANS_NEUTRAL_HOLD) {
        s_esc.pending_dir = 0;
        s_esc.pending_gval = 0;
        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_STOP) {
        car0107_esc_start_ramp(dir, gval);
        return;
    }

    if (s_esc.state == CAR0107_ESC_TRANSITION) {
        if (s_esc.trans_phase == CAR0107_TRANS_BRAKE ||
            s_esc.trans_phase == CAR0107_TRANS_NEUTRAL_HOLD) {
            s_esc.pending_dir = dir;
            s_esc.pending_gval = gval;
            return;
        }
        if (s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
            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;
    }

    /* 前进切后退：先 1000us 刹车 */
    if (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD && dir == CAR0107_ESC_DIR_BACK) {
        car0107_esc_begin_transition(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)
{
    (void)current;
    (void)target;
    return CAR0107_ESC_RAMP_STEP_RUN;
}

static void car0107_esc_apply_phone(int dir, int gval)
{
    if (gval < CAR0107_ESC_GVAL_MIN) {
        return;
    }

    car0107_esc_request(dir, gval);
    /* 不在此处瞬间写满 PWM；刹车/回中/慢爬均由 esc_tick 输出 */
}

static void car0107_phone_apply_merged(void)
{
    int fwd = s_phone_mode1_val;
    int back = s_phone_mode2_val;
    int fwd_val = car0107_phone_clamp_val(fwd);
    int back_val = car0107_phone_clamp_val(back);

    /* mode1/mode2 收到 val=0 或 <=52 都表示停下；以最近更新的通道为准 */
    if (s_phone_last_mode == 2) {
        if (back_val > 0) {
            car0107_esc_apply_phone(CAR0107_ESC_DIR_BACK, back_val);
        } else {
            car0107_esc_request_stop();
        }
        return;
    }
    if (s_phone_last_mode == 1) {
        if (fwd_val > 0) {
            car0107_esc_apply_phone(CAR0107_ESC_DIR_FORWARD, fwd_val);
        } else {
            car0107_esc_request_stop();
        }
        return;
    }

    if (fwd_val == 0 && back_val == 0) {
        car0107_esc_request_stop();
    }
}

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_STOP:
        s_esc.current_us = car0107_esc_ramp_toward(s_esc.current_us,
                                                   CAR0107_ESC_US_NEUTRAL,
                                                   CAR0107_ESC_RAMP_STEP_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));
        break;

    case CAR0107_ESC_TRANSITION:
        if (s_esc.trans_phase == CAR0107_TRANS_BRAKE) {
            car0107_esc_write_pwm(car0107_forward_brake_pwm());
            if (s_esc.brake_ticks > 0) {
                s_esc.brake_ticks--;
            }
            if (s_esc.brake_ticks <= 0) {
                s_esc.trans_phase = CAR0107_TRANS_NEUTRAL_HOLD;
                s_esc.neutral_hold_ticks = CAR0107_ESC_NEUTRAL_HOLD_TICKS;
                s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
                s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
            }
        } else if (s_esc.trans_phase == CAR0107_TRANS_NEUTRAL_HOLD) {
            s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
            if (s_esc.neutral_hold_ticks > 0) {
                s_esc.neutral_hold_ticks--;
            }
            if (s_esc.neutral_hold_ticks <= 0) {
                if (s_esc.pending_dir == 0) {
                    s_esc.state = CAR0107_ESC_STOP;
                    s_esc.cmd_dir = 0;
                    s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
                    s_esc.trans_phase = CAR0107_TRANS_BRAKE;
                } else {
                    s_esc.cmd_dir = s_esc.pending_dir;
                    s_esc.target_us = car0107_throttle_to_us(s_esc.pending_dir,
                                                             s_esc.pending_gval);
                    s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
                    s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
                }
                s_esc.pending_dir = 0;
                s_esc.pending_gval = 0;
            }
        } else if (s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
            s_esc.current_us = car0107_esc_ramp_toward(s_esc.current_us,
                                                       s_esc.target_us,
                                                       CAR0107_ESC_RAMP_STEP_RUN);
            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;
                s_esc.trans_phase = CAR0107_TRANS_BRAKE;
            }
        }
        break;

    default:
        break;
    }

    if (s_esc.state != CAR0107_ESC_TRANSITION ||
        s_esc.trans_phase != CAR0107_TRANS_BRAKE) {
        int snap = (s_esc.state == CAR0107_ESC_STOP);
        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_STOP;
    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_ticks = 0;
    s_esc.neutral_hold_ticks = 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)
{
    pthread_mutex_lock(&s_car0107_hw_mutex);
    car0107_reset_steering_state();
    car0107_apply_stop();
    /* 异常停止不算用户控制，不刷新 idle 保活计时 */
    pthread_mutex_unlock(&s_car0107_hw_mutex);
}

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]);

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

    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();

    if (mode == 1) {
        s_phone_last_mode = 1;
        s_phone_mode1_val = raw_val;
        car0107_phone_apply_merged();
        car0107_esc_tick();
        pthread_mutex_unlock(&s_car0107_hw_mutex);
        return;
    }
    if (mode == 2) {
        s_phone_last_mode = 2;
        s_phone_mode2_val = raw_val;
        car0107_phone_apply_merged();
        car0107_esc_tick();
        pthread_mutex_unlock(&s_car0107_hw_mutex);
        return;
    }

    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_esc_tick();
    car0107_speed_smooth_process();
    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];

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