//! 真机（HIL）评估后端：在台架上逐次试验一组增益。
//!
//! 与 [`SimEvaluator`](crate::backend::SimEvaluator) 实现同一个 [`Evaluator`]，
//! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动，
//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。
//!
//! # 安全设计
//!
//! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）；
//! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c，
//! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。
//!
//! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池
//! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。

use std::sync::Mutex;
use std::time::Duration;

use anyhow::Context as _;
use chassis_bench::{Abort, SafetyEnvelope, run_step_trial};
use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
use dji_esc_sim::{LoopLimits, Scenario, Trace};
use motor_core::{Disable as _, MotorFeedback, VelocityGains};
use thiserror::Error;

use crate::backend::Evaluator;
use crate::cost::{Objective, Score, Verdict};

/// 真机后端错误。
#[derive(Debug, Error)]
pub enum BenchError {
    /// 无法打开总线。
    #[error("cannot open CAN link: {0}")]
    Link(String),
    /// 电调总线启动失败。
    #[error("cannot start ESC bus: {0}")]
    Bus(String),
    /// 启动超时：目标电调没有反馈。
    #[error("esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)")]
    NoFeedback {
        /// 目标电调 ID。
        esc_id: u8,
        /// 等待时长（毫秒）。
        waited_ms: u64,
    },
    /// 搜索期间因安全事件中止且未要求继续。
    #[error("aborted by safety envelope: {reason}")]
    Aborted {
        /// 触发原因。
        reason: String,
    },
}

/// 真机评估配置。
#[derive(Debug, Clone)]
pub struct BenchConfig {
    /// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。
    pub esc: EscConfig,
    /// 总线运行参数。
    pub bus: BusOptions,
    /// 评估场景。
    pub scenario: Scenario,
    /// 安全包络。
    pub envelope: SafetyEnvelope,
    /// 硬约束与权重。
    pub objective: Objective,
    /// 试验之间的静默期（等轮子停稳）。
    pub between_trials: Duration,
    /// 命令租约续期节拍。
    pub lease_period: Duration,
    /// 启动时等待反馈的超时。
    pub startup_timeout: Duration,
    /// 安全检查触发后是否继续搜索（默认 false = 立即停）。
    pub continue_on_abort: bool,
}

impl BenchConfig {
    /// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。
    #[must_use]
    pub fn new(
        esc: EscConfig,
        scenario: Scenario,
        envelope: SafetyEnvelope,
        objective: Objective,
    ) -> Self {
        Self {
            esc,
            bus: BusOptions::default(),
            scenario,
            envelope,
            objective,
            between_trials: Duration::from_millis(500),
            lease_period: Duration::from_millis(5),
            startup_timeout: Duration::from_secs(5),
            continue_on_abort: false,
        }
    }
}

/// 真机评估器（持有总线与运行时）。
pub struct BenchEvaluator {
    session: Mutex<Session>,
    scenario: Scenario,
    envelope: SafetyEnvelope,
    objective: Objective,
    effort_limit_raw: i16,
    between_trials: Duration,
    lease_period: Duration,
    continue_on_abort: bool,
    /// 目前见过的最优轨迹（报告用）。
    best: Mutex<Option<(f64, Trace)>>,
    /// 中止记录（报告用）。
    aborts: Mutex<Vec<String>>,
    describe: String,
}

struct Session {
    runtime: tokio::runtime::Runtime,
    bus: Option<EscBusHandle>,
    motor: EscMotor,
}

impl BenchEvaluator {
    /// 打开链路、启动总线并等待目标电调反馈。
    ///
    /// # Errors
    ///
    /// 链路 / 总线启动失败，或启动超时（电调无反馈）。
    pub fn open(link: can_link::CanLink, config: BenchConfig) -> Result<Self, BenchError> {
        let esc_id = config.esc.esc_id;
        let limits =
            LoopLimits { current_limit_raw: config.esc.current_limit, model: config.esc.model };
        let runtime = tokio::runtime::Builder::new_multi_thread()
            .worker_threads(2)
            .enable_all()
            .build()
            .map_err(|e| BenchError::Bus(e.to_string()))?;

        let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone())
            .map_err(|e| BenchError::Bus(e.to_string()))?;
        let motor = bus
            .motor(esc_id)
            .ok_or_else(|| BenchError::Bus(format!("esc {esc_id} missing from the bus")))?
            .clone();

        let startup_timeout = config.startup_timeout;
        let ready = runtime.block_on(async {
            let deadline = tokio::time::Instant::now() + startup_timeout;
            loop {
                if motor.feedback_age().is_some_and(|age| age <= Duration::from_millis(100)) {
                    return true;
                }
                if tokio::time::Instant::now() >= deadline {
                    return false;
                }
                tokio::time::sleep(Duration::from_millis(20)).await;
            }
        });
        let evaluator = Self {
            session: Mutex::new(Session { runtime, bus: Some(bus), motor }),
            scenario: config.scenario,
            envelope: config.envelope,
            objective: config.objective,
            effort_limit_raw: limits.current_limit_raw,
            between_trials: config.between_trials,
            lease_period: config.lease_period,
            continue_on_abort: config.continue_on_abort,
            best: Mutex::new(None),
            aborts: Mutex::new(Vec::new()),
            describe: format!(
                "bench(esc={esc_id}, name={}, limit={} raw, target={} rpm, trial={:.1}s, gap={}ms)",
                config.esc.name,
                limits.current_limit_raw,
                config.scenario.target_rpm,
                config.scenario.total_s(),
                config.between_trials.as_millis()
            ),
        };
        if !ready {
            // 启动失败也必须走完整停机路径：先 0 电流，再关总线。
            evaluator.shutdown();
            return Err(BenchError::NoFeedback {
                esc_id,
                waited_ms: startup_timeout.as_millis() as u64,
            });
        }
        Ok(evaluator)
    }

    /// 最优轨迹（若有）。
    #[must_use]
    pub fn best_trace(&self) -> Option<Trace> {
        self.best
            .lock()
            .unwrap_or_else(std::sync::PoisonError::into_inner)
            .as_ref()
            .map(|(_, trace)| trace.clone())
    }

    /// 搜索期间的安全中止记录。
    #[must_use]
    pub fn aborts(&self) -> Vec<String> {
        self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).clone()
    }

    /// 停机并关闭总线（**必须**在结束前调用）。
    pub fn shutdown(&self) {
        let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
        let Some(bus) = session.bus.take() else {
            return;
        };
        for motor in bus.motors() {
            motor.disable();
        }
        session.runtime.block_on(async {
            tokio::time::sleep(Duration::from_millis(50)).await;
            bus.shutdown().await;
        });
    }

    /// 一次真机试验（含试验间隔静默）。
    fn run_trial(&self, gains: &VelocityGains) -> Score {
        let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
        let Session { runtime, motor, .. } = &mut *session;
        motor.set_velocity_gains(*gains);
        let scenario = self.scenario;
        let envelope = self.envelope.clone();
        let limit = self.effort_limit_raw;
        let lease = self.lease_period;
        let between = self.between_trials;

        let outcome = runtime.block_on(async {
            let trial =
                run_step_trial(motor, &scenario, &envelope, limit, lease, std::future::pending())
                    .await;
            // 试验间隔：让轮子真正停下来，再让下一次试验的起点一致。
            tokio::time::sleep(between).await;
            trial
        });

        if let Some(reason) = outcome.abort {
            let verdict = verdict_for(&reason);
            let message = format!("{}: {reason}", self.describe);
            self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).push(message);
            return Score::penalized(verdict);
        }

        let score = match crate::metrics::StepMetrics::from_trace(&outcome.trace, &self.scenario) {
            Some(metrics) => self.objective.score_metrics(metrics),
            None => Score::penalized(Verdict::Rejected),
        };
        // 记录目前最优的轨迹，供报告画图 / 复盘。
        let mut best = self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
        if best.as_ref().is_none_or(|(cost, _)| score.cost < *cost) {
            *best = Some((score.cost, outcome.trace));
        }
        score
    }
}

impl Drop for BenchEvaluator {
    fn drop(&mut self) {
        self.shutdown();
    }
}

impl Evaluator for BenchEvaluator {
    fn evaluate(&self, gains: &VelocityGains) -> Score {
        let score = self.run_trial(gains);
        if !score.verdict.is_ok() && !self.continue_on_abort {
            // 真机上出现安全事件时不"带伤继续搜索"：交给调用方在 sweep 之后
            // 检查 `aborts()` 并决定是否收工。这里只记录，不 panic。
            tracing::warn!(verdict = score.verdict.label(), "bench trial did not pass");
        }
        score
    }

    fn describe(&self) -> String {
        self.describe.clone()
    }
}

/// 安全事件 → 成本判定。
///
/// 过速 / 过温说明控制器失稳；掉线 / 超时 / 取消说明本次试验没做完，
/// 两者的结果都不可用，但原因不同——报告要能区分。
#[must_use]
pub fn verdict_for(abort: &Abort) -> Verdict {
    match abort {
        Abort::Overspeed { .. } | Abort::OverTemperature { .. } => Verdict::Overspeed,
        Abort::FeedbackLost { .. } | Abort::Timeout { .. } | Abort::Cancelled => Verdict::Rejected,
    }
}

/// 打开真机链路：给了 `interface` 走 SocketCAN，否则走 slcan 串口。
///
/// # Errors
///
/// 设备 / 接口打开失败（路径、权限、接口未 up、平台不支持）时返回错误。
pub fn open_link(
    device: &str,
    serial_baud: u32,
    can_speed: chassis_bench::CanSpeed,
    interface: Option<&str>,
) -> Result<can_link::CanLink, BenchError> {
    let result = match interface {
        Some(interface) => chassis_bench::open_socketcan(interface),
        None => chassis_bench::open_slcan(
            device,
            chassis_bench::default_slcan_options(serial_baud, can_speed),
        ),
    };
    result
        .with_context(|| match interface {
            Some(interface) => format!("打开 SocketCAN 接口 {interface} 失败（ip link / 权限？）"),
            None => format!("打开 slcan 设备 {device} 失败（路径 / 权限 / 占用？）"),
        })
        .map_err(|e| BenchError::Link(e.to_string()))
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::cost::Weights;

    fn config() -> BenchConfig {
        let mut esc = EscConfig::gm3508("bench", 1, 1.0);
        esc.current_limit = 1500;
        let scenario = Scenario::ramp_and_hold(500.0, 0.05, 0.15);
        let objective = Objective::new(
            Weights::default(),
            scenario,
            LoopLimits { current_limit_raw: esc.current_limit, model: esc.model },
        );
        let envelope = SafetyEnvelope::new()
            .with_overspeed(1200.0, Duration::from_millis(60))
            .with_temperature_max(70)
            .with_feedback_timeout(Duration::from_millis(200))
            .with_trial_timeout(Duration::from_secs(2));
        BenchConfig::new(esc, scenario, envelope, objective)
    }

    #[test]
    fn config_defaults_are_safe() {
        let config = config();
        assert_eq!(config.bus.period, Duration::from_millis(1));
        assert_eq!(config.bus.command_timeout, Duration::from_millis(50));
        assert!(!config.continue_on_abort, "默认必须在安全事件后收工");
        assert!(config.between_trials >= Duration::from_millis(200));
        assert_eq!(config.lease_period, Duration::from_millis(5), "续租必须远密于 50ms 租约");
    }

    #[test]
    fn every_abort_maps_to_a_definite_verdict() {
        let cases = [
            Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 },
            Abort::OverTemperature { temperature_c: 90, limit_c: 70 },
            Abort::FeedbackLost { esc_id: 1, age_ms: None, timeout_ms: 200 },
            Abort::Timeout { elapsed_ms: 6000 },
            Abort::Cancelled,
        ];
        for abort in cases {
            let verdict = verdict_for(&abort);
            assert!(!verdict.is_ok(), "{abort:?} 不得被判为达标");
        }
        assert_eq!(
            verdict_for(&Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 }),
            Verdict::Overspeed
        );
        assert_eq!(verdict_for(&Abort::Cancelled), Verdict::Rejected);
    }

    #[test]
    fn link_open_failures_are_errors() {
        // 不存在的设备：必须返回 Err 而不是 panic（真机前最常见的失败模式）。
        let result =
            open_link("/definitely/not/a/device", 2_000_000, chassis_bench::CanSpeed::M1, None);
        assert!(result.is_err(), "不存在的串口设备必须报错");
        let missing_interface =
            open_link("ignored", 2_000_000, chassis_bench::CanSpeed::M1, Some("can-not-real"));
        assert!(missing_interface.is_err(), "不存在的 CAN 接口必须报错");
    }
}