//! 守护进程端到端测试：`--mock` 同构栈（配置 → EscBus → 通道 → 仿真电机
//! → 控制循环 → 麦轮运动学），全平台可跑。

#![allow(clippy::unwrap_used, clippy::expect_used)]

use std::time::Duration;

use chassisd::config::Config;
use chassisd::controller::{ChassisController, wrap_wheels};
use dji_esc::{CanLink, EscBus, EscBusHandle, spawn_plant};
use mecanum::Twist;
use motor_core::MotorFeedback;
use tokio::sync::watch;
use tokio::time::sleep;

/// 搭建与 `chassisd --mock` 完全同构的软件栈。
///
/// `command_timeout_ms` 由调用方指定：稳态测试用大值（模拟持续刷新的
/// 指令源），超时测试用配置原值。
fn stack(config: &Config, command_timeout_ms: u64) -> (EscBusHandle, watch::Sender<Twist>) {
    let (bus_side, plant_side) = CanLink::pair(512);
    let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
    spawn_plant(plant_side, &esc_ids, config.bus_options().period);

    let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options()).unwrap();

    let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
    let motors: [dji_esc::EscMotor; 4] =
        bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
    let gears: [f32; 4] = config
        .motors
        .iter()
        .map(|m| m.gear_ratio)
        .collect::<Vec<_>>()
        .try_into()
        .unwrap_or_else(|_| unreachable!("exactly 4 motors"));
    let controller = ChassisController::new(
        config.kinematics(),
        wrap_wheels(motors, gears),
        (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega),
        Duration::from_millis(command_timeout_ms),
    );
    controller.spawn(twist_rx, config.bus_options().period);
    (bus, twist_tx)
}

async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool {
    let mut waited = 0u64;
    while waited < total_ms {
        if cond() {
            return true;
        }
        sleep(Duration::from_millis(5)).await;
        waited += 5;
    }
    cond()
}

fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] {
    bus.motors()
        .iter()
        .map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio)
        .collect::<Vec<_>>()
        .try_into()
        .unwrap()
}

/// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。
const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762);

#[tokio::test(start_paused = true)]
async fn forward_command_drives_all_wheels_equally() {
    let config = Config::mock_default();
    let (bus, twist_tx) = stack(&config, 10_000);

    twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
    let target = RPM_PER_MPS;
    let ok = wait_until(3000, || {
        wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target)
    })
    .await;
    let rpm = wheel_rpms(&bus);
    assert!(ok, "未收敛到 {target} rpm: {rpm:?}");

    // 停车：指令归零后各轮减速到停。
    twist_tx.send_replace(Twist::ZERO);
    let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
    let rpm = wheel_rpms(&bus);
    assert!(stopped, "未停车: {rpm:?}");
    bus.shutdown().await;
}

#[tokio::test(start_paused = true)]
async fn rotation_command_respects_wheel_signs() {
    let config = Config::mock_default();
    let (bus, twist_tx) = stack(&config, 10_000);

    twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO });
    // a·ω = (0.2 + 0.25) m/s 的轮线速度。
    let target = 0.45 * RPM_PER_MPS;
    let ok = wait_until(3000, || {
        let rpm = wheel_rpms(&bus);
        // 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。
        [
            (rpm[0] + target).abs(),
            (rpm[1] - target).abs(),
            (rpm[2] + target).abs(),
            (rpm[3] - target).abs(),
        ]
        .iter()
        .all(|e| *e < 0.05 * target)
    })
    .await;
    let rpm = wheel_rpms(&bus);
    assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}");
    bus.shutdown().await;
}

#[tokio::test(start_paused = true)]
async fn command_timeout_stops_chassis() {
    let config = Config::mock_default();
    // 指令超时使用配置原值（200ms）。
    let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms);

    // 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。
    twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
    let moving =
        wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)).await;
    assert!(moving, "指令后底盘应当运动");

    sleep(Duration::from_millis(400)).await; // 超过 200ms 超时
    let stopped = wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
    let rpm = wheel_rpms(&bus);
    assert!(stopped, "指令超时后未停车: {rpm:?}");
    bus.shutdown().await;
}

#[tokio::test(start_paused = true)]
async fn speed_limits_scale_motion() {
    let mut config = Config::mock_default();
    config.chassis.max_vx = 1.0; // 限速 1 m/s
    let (bus, twist_tx) = stack(&config, 10_000);

    twist_tx.send_replace(Twist {
        vx: 3.0, // 超限指令
        vy: 2.0,
        omega: 0.0,
    });
    // 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy：
    // FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。
    let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS;
    let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS;
    let tol = 0.08 * diag_fast;
    let ok = wait_until(3000, || {
        let rpm = wheel_rpms(&bus);
        [
            (rpm[0] - diag_slow).abs(),
            (rpm[1] - diag_fast).abs(),
            (rpm[2] - diag_fast).abs(),
            (rpm[3] - diag_slow).abs(),
        ]
        .iter()
        .all(|e| *e < tol)
    })
    .await;
    let rpm = wheel_rpms(&bus);
    assert!(ok, "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}");
    bus.shutdown().await;
}