//! 语义一致性：同步评估 harness 与异步 `EscBus` 闭环必须给出同一个答案。
//!
//! 自动整定跑在 [`dji_esc_sim::simulate`]（同步、无通道）上，而实车跑的是
//! `EscBus` 的 1 kHz 发送循环。若两者的限幅 / 物理帧 / 控制器语义漂移，
//! 整定出来的增益就只是"对仿真有效"。本测试把两条路径放在同一组参数与
//! 同一条场景下比较稳态与 effort。
//!
//! 允许的差异来自**离散化细节**（异步侧每拍实测 `dt`、反馈比命令晚一拍），
//! 因此比较的是稳态量级而不是逐点相等。

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

use std::time::Duration;

use dji_esc::{BusOptions, CanLink, EscBus, EscConfig};
use dji_esc_sim::{LoopLimits, PlantParams, Scenario, simulate, spawn_plant};
use motor_core::{MotorFeedback, VelocityDrive, VelocityGains};
use tokio::time::sleep;

/// 与 `PlantParams::gm3508` 匹配的增益（ff = 16384 / 9254）。
const GAINS: VelocityGains = VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0);
const TARGET_RPM: f32 = 3000.0;
const HOLD_MS: u64 = 2000;
/// 稳态采样窗口（虚拟时间，毫秒）。
const SAMPLE_FROM_MS: u64 = 1500;

/// 同步路径：最后 500 ms 的稳态统计。
fn sync_steady_state() -> (f32, f32) {
    let scenario = Scenario::step(TARGET_RPM, HOLD_MS as f32 / 1000.0);
    let trace = simulate(GAINS, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508())
        .expect("simulation must succeed");
    let window = trace.window(SAMPLE_FROM_MS as f32 / 1000.0, HOLD_MS as f32 / 1000.0);
    let mean_effort =
        window.effort_raw().iter().map(|&raw| f32::from(raw)).sum::<f32>() / window.len() as f32;
    (window.mean_rpm(), mean_effort)
}

/// 异步路径：`EscBus` + 假电调，逐拍续租直到 2 秒（虚拟时间）。
async fn async_steady_state() -> (f32, f32) {
    let (bus_link, plant_link) = CanLink::pair(512);
    let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
    let mut cfg = EscConfig::gm3508("front_left", 1, 1.0);
    cfg.kp = GAINS.kp;
    cfg.ki = GAINS.ki;
    cfg.kd = GAINS.kd;
    cfg.feedforward = GAINS.feedforward;
    cfg.current_limit = LoopLimits::gm3508().current_limit_raw;
    let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).expect("bus must start");
    let motor = bus.motor(1).expect("motor 1 exists").clone();

    // 命令是租约：逐拍重发（5 ms 节拍模拟 1 kHz 控制循环）。
    let mut waited = 0u64;
    let mut effort_sum = 0.0f32;
    let mut rpm_sum = 0.0f32;
    let mut samples = 0u32;
    while waited < HOLD_MS {
        motor.set_velocity(TARGET_RPM);
        if waited >= SAMPLE_FROM_MS {
            let state = motor.state();
            effort_sum += f32::from(state.effort_raw);
            rpm_sum += state.rotor_speed_rpm;
            samples += 1;
        }
        sleep(Duration::from_millis(5)).await;
        waited += 5;
    }

    plant.abort();
    bus.shutdown().await;
    (rpm_sum / samples as f32, effort_sum / samples as f32)
}

#[tokio::test(start_paused = true)]
async fn synchronous_harness_matches_async_closed_loop() {
    let (sync_rpm, sync_effort) = sync_steady_state();
    let (async_rpm, async_effort) = async_steady_state().await;

    // 稳态转速：两条路径都应收敛到目标附近（理想模型 + 精确前馈 → 稳态无差）。
    let rpm_tolerance = 0.05 * TARGET_RPM;
    assert!(
        (sync_rpm - async_rpm).abs() < rpm_tolerance,
        "steady rpm mismatch: sync {sync_rpm}, async {async_rpm}"
    );
    assert!(
        (sync_rpm - TARGET_RPM).abs() < rpm_tolerance,
        "sync steady state off target: {sync_rpm}"
    );
    assert!(
        (async_rpm - TARGET_RPM).abs() < rpm_tolerance,
        "async steady state off target: {async_rpm}"
    );

    // effort：前馈主导，两条路径量级必须接近（±20% 或 ±300 raw 取大者）。
    let effort_tolerance = (0.2 * sync_effort.abs()).max(300.0);
    assert!(
        (sync_effort - async_effort).abs() < effort_tolerance,
        "steady effort mismatch: sync {sync_effort}, async {async_effort}"
    );
}