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

//! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。
//!
//! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务：
//!
//! - 接收：解码反馈帧，更新各电机共享状态（含多圈累计与方向修正）；
//! - 发送：按 `period` 节拍为每台电机求值当前命令（速度/位置环 PID / 直发
//!   effort），按 `0x200`/`0x1FF` 分组组帧发送。
//!
//! 电机离线（从未收到反馈或反馈超时）时强制 0 输出并复位控制器。
//! 命令模式切换时同样复位控制器，避免积分冲击。

use std::sync::{Arc, Mutex, MutexGuard};
use std::time::Duration;

use can::{CanFrame, CanLink};
use motor_protocol::common::{
    EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame,
};
use motor_protocol::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, TurnTracker};
use thiserror::Error;
use tokio::sync::{mpsc, watch};
use tokio::task::JoinHandle;
use tokio::time::{Instant, Interval, MissedTickBehavior};
use tracing::{debug, info, warn};

use motor::{
    Command, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorState, PolicyInput,
    PolicySample, PositionDrive, VelocityDrive,
};

/// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌，
/// 状态本身仍一致，驱动机不应因此停摆）。
fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
    mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
}

/// 单台电机的配置。
#[derive(Debug, Clone)]
pub struct EscConfig {
    /// 便于日志辨认的名字（如 `front_left`）。
    pub name: String,
    /// 电调 ID（1–8，即 CAN 拨码）。
    pub esc_id: u8,
    /// 电调型号。
    pub model: EscModel,
    /// 减速比（电机转子 : 输出轴）。
    ///
    /// 驱动层**不使用**此字段；由上层 `crate::OutputShaft` 应用。
    /// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。
    pub gear_ratio: f32,
    /// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转，
    /// 上层永远以"输出轴正转 = 前进方向"工作。
    pub direction: f32,
    /// effort 限幅（raw，`1..=型号满量程`）。
    pub current_limit: i16,
}

impl EscConfig {
    /// GM3508 + C620 的常用配置。`kp/ki/ff` 为起步值，需按整车整定。
    #[must_use]
    pub fn gm3508(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
        Self {
            name: name.into(),
            esc_id,
            model: EscModel::C620,
            // 3591 / 187 ≈ 19.203
            gear_ratio: 3591.0 / 187.0,
            direction,
            current_limit: 12_000,
        }
    }

    /// M2006 + C610（拨弹轮）的常用配置。
    ///
    /// 减速比按 P36 起步（36.0），接硬件前需核对；电流标度见 [`EscModel::C610`]。
    #[must_use]
    pub fn m2006(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
        Self {
            name: name.into(),
            esc_id,
            model: EscModel::C610,
            // M2006 P36 减速比（接入前核对）
            gear_ratio: 36.0,
            direction,
            current_limit: 8_000,
        }
    }

    fn validate(&self) -> Result<(), String> {
        if command_frame_id(self.esc_id).is_none() {
            return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
        }
        if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
            return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio));
        }
        // 逐位比较：direction 只允许精确的 ±1.0。
        let dir_bits = self.direction.to_bits();
        if dir_bits != 1.0f32.to_bits() && dir_bits != (-1.0f32).to_bits() {
            return Err(format!("direction must be +1.0 or -1.0, got {}", self.direction));
        }
        if !(1..=self.model.max_current_raw()).contains(&self.current_limit) {
            return Err(format!(
                "current_limit {} out of range 1..={} for {:?}",
                self.current_limit,
                self.model.max_current_raw(),
                self.model
            ));
        }
        Ok(())
    }
}

/// 总线运行参数。
#[derive(Debug, Clone)]
pub struct BusOptions {
    /// 控制节拍（C620 期望 1kHz 命令）。
    pub period: Duration,
    /// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。
    pub feedback_timeout: Duration,
    /// **命令租约**：上层超过该时长未重新下发命令 → 视为 `Disabled`（0 输出）。
    ///
    /// 命令是**租约**而非一次性设定值：`set_velocity` / `set_effort_raw` /
    /// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发
    /// （`chassisd` 的 1kHz 循环天然满足；台架脚本每拍重发即可），
    /// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。
    ///
    /// 设置为 `Duration::MAX` 可退化为纯设定值语义（仅建议在无失控风险的台架场景）。
    pub command_timeout: Duration,
}

impl Default for BusOptions {
    fn default() -> Self {
        Self {
            period: Duration::from_millis(1),
            feedback_timeout: Duration::from_millis(100),
            // 1kHz 下约 50 拍未刷新即失效。
            command_timeout: Duration::from_millis(50),
        }
    }
}

/// 启动错误。
#[derive(Debug, Clone, PartialEq, Eq, Error)]
pub enum EscBusError {
    /// 两台电机配置了同一电调 ID。
    #[error("duplicate ESC id {0}")]
    DuplicateEscId(u8),
    /// 电机配置非法（原因见 `reason`）。
    #[error("invalid config for motor {name}: {reason}")]
    InvalidConfig {
        /// 电机名。
        name: String,
        /// 非法原因。
        reason: String,
    },
    /// 一台电机都没有配置。
    #[error("no motors configured")]
    NoMotors,
}

/// 单电机共享状态（驱动任务与 [`EscMotor`] 句柄之间）。
struct MotorShared {
    cfg: EscConfig,
    state: Mutex<MotorState>,
    cmd: Mutex<Command>,
    /// 命令写入时刻（看门狗用）。
    cmd_at: Mutex<Option<Instant>>,
    turn: Mutex<TurnTracker>,
    last_feedback: Mutex<Option<Instant>>,
    offline_warned: std::sync::atomic::AtomicBool,
}

impl MotorShared {
    fn new(cfg: EscConfig) -> Self {
        Self {
            cfg,
            state: Mutex::new(MotorState::default()),
            cmd: Mutex::new(Command::Disabled),
            cmd_at: Mutex::new(None),
            turn: Mutex::new(TurnTracker::new()),
            last_feedback: Mutex::new(None),
            offline_warned: std::sync::atomic::AtomicBool::new(false),
        }
    }

    fn store_command(&self, cmd: Command) {
        *lock(&self.cmd) = cmd;
        *lock(&self.cmd_at) = Some(Instant::now());
    }

    /// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。
    fn apply_feedback(&self, fb: EscFeedback, now: Instant) {
        let esc_rev = lock(&self.turn).update(fb.angle_raw);
        let dir = self.cfg.direction;
        {
            let mut st = lock(&self.state);
            st.rotor_angle_raw = fb.angle_raw;
            st.rotor_revolutions = dir * esc_rev;
            st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm);
            st.effort_raw = (dir * f32::from(fb.current_raw)) as i16;
            st.temperature_c = fb.temperature_c;
            st.feedback_count = st.feedback_count.saturating_add(1);
        }
        *lock(&self.last_feedback) = Some(now);
        self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed);
    }

    /// Produce one policy sample while enforcing feedback and command leases.
    fn sample(
        &self,
        feedback_timeout: Duration,
        command_timeout: Duration,
        now: Instant,
    ) -> PolicySample {
        let online = matches!(
            *lock(&self.last_feedback),
            Some(t) if now.saturating_duration_since(t) <= feedback_timeout
        );
        if !online && !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
            warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current");
        }
        let fresh = matches!(
            *lock(&self.cmd_at),
            Some(t) if now.saturating_duration_since(t) <= command_timeout
        );
        let command = if online && fresh { *lock(&self.cmd) } else { Command::Disabled };
        PolicySample { id: self.cfg.esc_id, state: *lock(&self.state), command, online }
    }
}

/// 电机的公共句柄：可克隆，跨任务共享。
///
/// # 命令租约
///
/// 下发命令（[`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] /
/// [`PositionDrive::set_position`]）即续租，租期见 [`BusOptions::command_timeout`]。
/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电（0 输出），
/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。
#[derive(Clone)]
pub struct EscMotor {
    shared: Arc<MotorShared>,
}

impl EscMotor {
    /// 电机名。
    #[must_use]
    pub fn name(&self) -> &str {
        &self.shared.cfg.name
    }

    /// 电机配置。
    #[must_use]
    pub fn config(&self) -> &EscConfig {
        &self.shared.cfg
    }

    /// 当前命令。
    #[must_use]
    pub fn command(&self) -> Command {
        *lock(&self.shared.cmd)
    }

    /// 最近一帧反馈的时刻（诊断用）。
    #[must_use]
    pub fn last_feedback(&self) -> Option<Instant> {
        *lock(&self.shared.last_feedback)
    }
}

impl MotorFeedback for EscMotor {
    fn id(&self) -> u8 {
        self.shared.cfg.esc_id
    }

    fn state(&self) -> MotorState {
        *lock(&self.shared.state)
    }

    fn feedback_age(&self) -> Option<std::time::Duration> {
        let t = *lock(&self.shared.last_feedback);
        Some(Instant::now().saturating_duration_since(t?))
    }
}

impl VelocityDrive for EscMotor {
    /// 设定目标转子转速（**租约**，见 [`EscMotor`] 的命令租约说明）。
    fn set_velocity(&self, rotor_rpm: f32) {
        self.shared.store_command(Command::Velocity(rotor_rpm));
    }
}

impl EffortDrive for EscMotor {
    /// 直发开环 raw（**租约**，见 [`EscMotor`] 的命令租约说明）。
    fn set_effort_raw(&self, raw: i16) {
        self.shared.store_command(Command::EffortRaw(raw));
    }
}

impl PositionDrive for EscMotor {
    /// 设定目标转子多圈位置（**租约**，见 [`EscMotor`] 的命令租约说明）。
    fn set_position(&self, rotor_rev: f32) {
        self.shared.store_command(Command::Position(rotor_rev));
    }
}

impl Disable for EscMotor {
    /// 停用（0 输出）；同样受租约保护——上层若已死，租约到期后依旧归零。
    fn disable(&self) {
        self.shared.store_command(Command::Disabled);
    }
}

/// 运行中的电调总线。
pub struct EscBus;

impl EscBus {
    /// 启动收发任务，返回句柄。
    ///
    /// # Errors
    ///
    /// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。
    pub fn spawn<P: EffortPolicy>(
        link: CanLink,
        configs: Vec<EscConfig>,
        options: BusOptions,
        policy: P,
    ) -> Result<EscBusHandle<P>, EscBusError> {
        if configs.is_empty() {
            return Err(EscBusError::NoMotors);
        }
        let mut motors = Vec::with_capacity(configs.len());
        let mut seen = std::collections::HashSet::new();
        for cfg in configs {
            cfg.validate()
                .map_err(|reason| EscBusError::InvalidConfig { name: cfg.name.clone(), reason })?;
            if !seen.insert(cfg.esc_id) {
                return Err(EscBusError::DuplicateEscId(cfg.esc_id));
            }
            motors.push(EscMotor { shared: Arc::new(MotorShared::new(cfg)) });
        }

        let (shutdown_tx, shutdown_rx) = watch::channel(false);
        let CanLink { outgoing_tx, incoming_rx } = link;
        let motors = Arc::new(motors);
        let policy = Arc::new(Mutex::new(policy));
        let rx_handle =
            tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone()));
        let tx_handle = tokio::spawn(tx_loop(
            outgoing_tx,
            Arc::clone(&motors),
            Arc::clone(&policy),
            options,
            shutdown_rx,
        ));

        Ok(EscBusHandle { motors, policy, shutdown_tx, tasks: vec![rx_handle, tx_handle] })
    }
}

/// `EscBus` 的控制句柄。
pub struct EscBusHandle<P> {
    motors: Arc<Vec<EscMotor>>,
    policy: Arc<Mutex<P>>,
    shutdown_tx: watch::Sender<bool>,
    tasks: Vec<JoinHandle<()>>,
}

impl<P: EffortPolicy> EscBusHandle<P> {
    /// 按电调 ID 取电机句柄。
    #[must_use]
    pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> {
        self.motors.iter().find(|m| m.id() == esc_id)
    }

    /// 全部电机（按配置顺序）。
    #[must_use]
    pub fn motors(&self) -> &[EscMotor] {
        &self.motors
    }

    /// Mutate the composed control policy between bus ticks.
    pub fn with_policy<R>(&self, update: impl FnOnce(&mut P) -> R) -> R {
        update(&mut lock(&self.policy))
    }

    /// 停止收发任务并等待退出。
    pub async fn shutdown(self) {
        let _ = self.shutdown_tx.send(true);
        for task in self.tasks {
            let _ = task.await;
        }
    }
}

async fn rx_loop(
    mut incoming: mpsc::Receiver<CanFrame>,
    motors: Arc<Vec<EscMotor>>,
    mut shutdown: watch::Receiver<bool>,
) {
    loop {
        let frame = tokio::select! {
            _ = shutdown.changed() => break,
            frame = incoming.recv() => match frame {
                Some(frame) => frame,
                None => break,
            },
        };
        match decode_feedback(&frame) {
            Ok((esc_id, fb)) => {
                if let Some(motor) = motors.iter().find(|m| m.id() == esc_id) {
                    motor.shared.apply_feedback(fb, Instant::now());
                }
            }
            Err(e) => {
                debug!(error = %e, id = frame.id, "ignore non-feedback frame");
            }
        }
    }
}

async fn tx_loop<P: EffortPolicy>(
    outgoing: mpsc::Sender<CanFrame>,
    motors: Arc<Vec<EscMotor>>,
    policy: Arc<Mutex<P>>,
    options: BusOptions,
    mut shutdown: watch::Receiver<bool>,
) {
    let nominal_dt = options.period.as_secs_f32();
    let mut ticker = tick_every(options.period);
    let mut last_tick = Instant::now();
    loop {
        tokio::select! {
            _ = shutdown.changed() => break,
            _ = ticker.tick() => {}
        }
        let now = Instant::now();
        let raw_dt = now.saturating_duration_since(last_tick).as_secs_f32();
        last_tick = now;
        let dt = raw_dt.clamp(nominal_dt * 0.5, nominal_dt * 4.0);
        let mut samples: [PolicySample; 8] = std::array::from_fn(|index| PolicySample {
            id: u8::try_from(index + 1).unwrap_or(0),
            state: MotorState::default(),
            command: Command::Disabled,
            online: false,
        });
        for motor in motors.iter() {
            let sample =
                motor.shared.sample(options.feedback_timeout, options.command_timeout, now);
            if let Some(slot) = samples.get_mut(usize::from(sample.id.saturating_sub(1))) {
                *slot = sample;
            }
        }
        let efforts = lock(&policy).step(&PolicyInput { samples, dt });
        for (frame_id, group_start) in
            [(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)]
        {
            let group: Vec<&EscMotor> = motors
                .iter()
                .filter(|m| (group_start..group_start + 4).contains(&m.id()))
                .collect();
            if group.is_empty() {
                continue;
            }
            let mut currents = [0i16; 4];
            for motor in &group {
                let slot = usize::from(motor.id() - group_start);
                let policy_index = usize::from(motor.id().saturating_sub(1));
                let physical = efforts.get(policy_index).copied().unwrap_or(0);
                let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16;
                currents[slot] = clamp_current(
                    motor.shared.cfg.model,
                    directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit),
                );
            }
            if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() {
                info!("can link closed, tx loop exiting");
                return;
            }
        }
    }
}

fn tick_every(period: Duration) -> Interval {
    let mut ticker = tokio::time::interval(period);
    // 错过节拍后直接跳到当前节拍，避免补发帧风暴。
    ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
    ticker
}

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

    #[test]
    fn config_validate_rejects_bad_values() {
        let mut cfg = EscConfig::gm3508("m", 1, 1.0);

        cfg.esc_id = 0;
        assert!(cfg.validate().is_err());
        cfg.esc_id = 9;
        assert!(cfg.validate().is_err());
        cfg.esc_id = 5;

        cfg.gear_ratio = 0.0;
        assert!(cfg.validate().is_err());
        cfg.gear_ratio = 19.2;

        cfg.direction = 0.5;
        assert!(cfg.validate().is_err());
        cfg.direction = -1.0;

        cfg.current_limit = 0;
        assert!(cfg.validate().is_err());
        cfg.current_limit = 16385;
        assert!(cfg.validate().is_err());
        cfg.current_limit = 12_000;

        assert!(cfg.validate().is_ok());
    }

    #[test]
    fn gm3508_defaults_are_sane() {
        let cfg = EscConfig::gm3508("front_left", 1, -1.0);
        assert_eq!(cfg.model, EscModel::C620);
        assert!((cfg.gear_ratio - 19.2).abs() < 0.01);
        assert_eq!(cfg.current_limit, 12_000);
        assert_eq!(cfg.direction.to_bits(), (-1.0f32).to_bits());
        assert!(cfg.validate().is_ok());
    }

    #[test]
    fn m2006_defaults_are_sane() {
        let cfg = EscConfig::m2006("feeder", 5, 1.0);
        assert_eq!(cfg.model, EscModel::C610);
        assert!((cfg.gear_ratio - 36.0).abs() < 0.01);
        assert!(cfg.validate().is_ok());
    }
}