//! `chassis.toml` 配置模型：解析、语义校验与到驱动配置的映射。
//!
//! 本 crate 是**配置 schema 的唯一来源**：`chassisd`（守护进程）与
//! `chassis-tune`（自动整定）读同一份类型定义，因此整定工具写回的
//! `[[motor]]` 片段必然能被守护进程读回，不需要两份 schema 手工同步。
//!
//! 全部类型都实现 `Serialize`，所以 [`Config::to_toml_string`] 可以把
//! （可能已被整定工具改写过的）配置原样落盘。
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]

use std::time::Duration;

use dji_esc::{BusOptions, EscConfig, EscModel};
use mecanum::Mecanum;
use motor_core::VelocityGains;
use serde::{Deserialize, Serialize};
use thiserror::Error;

/// 配置错误。
#[derive(Debug, Clone, PartialEq, Eq, Error)]
pub enum ConfigError {
    /// TOML 语法 / 结构错误。
    #[error("failed to parse config: {0}")]
    Parse(String),
    /// 语义校验失败。
    #[error("invalid config: {0}")]
    Invalid(String),
}

/// 顶层配置。
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)]
pub struct Config {
    /// CAN 配置。
    pub can: CanConfig,
    /// 底盘控制参数。
    #[serde(default)]
    pub chassis: ChassisConfig,
    /// 麦轮几何。
    pub mecanum: MecanumConfig,
    /// 四电机配置，顺序固定：前左、前右、后左、后右。
    #[serde(rename = "motor")]
    pub motors: Vec<MotorConfig>,
}

/// CAN 接口配置（双 CAN 总线 seam）。
///
/// 底盘与功能总线物理隔离，**每条总线只允许一种协议族**（见 [`ProtocolFamily`]）。
#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)]
pub struct CanConfig {
    /// 底盘总线 SocketCAN 接口名（如 `can0`）——C620 驱动轮，ESC 电流协议族。
    pub interface: String,
    /// 功能总线（云台 / 拨弹）：与底盘物理隔离。
    ///
    /// 预留 seam；填写后由上层功能模块（gimbal / feeder）使用。
    /// 典型：`can1`，承载 GM6020 云台 + M2006/C610 拨弹。
    #[serde(default)]
    pub function: Option<FunctionBusConfig>,
}

/// 功能总线配置（双 CAN 之二）。
#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)]
pub struct FunctionBusConfig {
    /// SocketCAN 接口名（如 `can1`）。
    pub interface: String,
}

/// 协议族——同一总线上**不得混用**。
///
/// C620/C610（电流）与 GM6020（电压）的控制/反馈帧 ID 空间重叠：
/// `0x1FF` 既可作 C620 5–8 电流也可作 GM6020 1–4 电压；`0x205` 同理。
/// 双 CAN 物理隔离是首选消解手段；单总线时必须保证同族。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum ProtocolFamily {
    /// C620/C610 电流协议（驱动轮、拨弹轮）。
    Esc,
    /// GM6020 电压协议（云台）。
    Gm6020,
}

impl ProtocolFamily {
    /// 电调型号所属协议族。
    #[must_use]
    pub const fn of_esc_model(model: EscModelSerde) -> Self {
        match model {
            EscModelSerde::C620 | EscModelSerde::C610 => Self::Esc,
        }
    }
}

/// 底盘控制参数（全部可省略，采用默认值）。
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
#[serde(deny_unknown_fields, default)]
pub struct ChassisConfig {
    /// 控制循环周期（毫秒）。
    pub control_period_ms: u64,
    /// 上层指令超时（毫秒）：超时后目标速度归零。
    pub command_timeout_ms: u64,
    /// 电机反馈离线阈值（毫秒）。
    pub feedback_timeout_ms: u64,
    /// 驱动层**命令租约**（毫秒）：控制循环超过该时长未刷新电机命令 →
    /// 驱动强制 0 电流（防控制任务死亡后旧指令持续输出）。
    ///
    /// 必须 ≥ `control_period_ms`；默认 1kHz 下约 50 拍。
    pub motor_command_timeout_ms: u64,
    /// 最大前进 / 横移速度（m/s）。
    pub max_vx: f32,
    /// 最大横移速度（m/s）。
    pub max_vy: f32,
    /// 最大角速度（rad/s）。
    pub max_omega: f32,
}

impl Default for ChassisConfig {
    fn default() -> Self {
        Self {
            control_period_ms: 1,
            command_timeout_ms: 200,
            feedback_timeout_ms: 100,
            motor_command_timeout_ms: 50,
            max_vx: 3.0,
            max_vy: 3.0,
            max_omega: 6.0,
        }
    }
}

/// 麦轮几何（米）。
#[derive(Debug, Clone, Copy, PartialEq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)]
pub struct MecanumConfig {
    /// 前后轮轴线到中心的纵向半距。
    pub half_length: f32,
    /// 左右轮到中心的横向半距。
    pub half_width: f32,
    /// 车轮半径。
    pub wheel_radius: f32,
}

/// 单电机配置。
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)]
pub struct MotorConfig {
    /// 便于日志辨认的名字（`front_left` 等）。
    pub name: String,
    /// 电调 ID（1–8）。
    pub esc_id: u8,
    /// 电调型号（`C620` / `C610`）。
    #[serde(default = "default_model")]
    pub model: EscModelSerde,
    /// 安装方向：`1` 或 `-1`。
    #[serde(default = "default_direction")]
    pub direction: f32,
    /// 减速比（GM3508 ≈ 19.203）。
    pub gear_ratio: f32,
    /// 电流限幅（raw）。
    #[serde(default = "default_current_limit")]
    pub current_limit: i16,
    /// 速度环 kp。
    #[serde(default)]
    pub kp: f32,
    /// 速度环 ki。
    #[serde(default)]
    pub ki: f32,
    /// 速度环 kd。
    #[serde(default)]
    pub kd: f32,
    /// 速度前馈（raw/rpm）。
    #[serde(default)]
    pub feedforward: f32,
}

fn default_model() -> EscModelSerde {
    EscModelSerde::C620
}

fn default_direction() -> f32 {
    1.0
}

fn default_current_limit() -> i16 {
    12_000
}

/// 配置文件中的电调型号。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize, Serialize)]
#[serde(deny_unknown_fields, rename_all = "UPPERCASE")]
pub enum EscModelSerde {
    /// C620（GM3508）。
    C620,
    /// C610（M2006）。
    C610,
}

impl From<EscModelSerde> for EscModel {
    fn from(value: EscModelSerde) -> Self {
        match value {
            EscModelSerde::C620 => Self::C620,
            EscModelSerde::C610 => Self::C610,
        }
    }
}

impl MotorConfig {
    /// 速度环增益组（可整定参数）。
    #[must_use]
    pub fn velocity_gains(&self) -> VelocityGains {
        VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward)
    }

    /// 覆写速度环增益组，其余字段（限流、方向、减速比）保持不变。
    pub fn set_velocity_gains(&mut self, gains: VelocityGains) {
        self.kp = gains.kp;
        self.ki = gains.ki;
        self.kd = gains.kd;
        self.feedforward = gains.feedforward;
    }
}

impl Config {
    /// 从 TOML 文本解析并校验。
    ///
    /// # Errors
    ///
    /// 解析失败或语义校验失败（电机数量、几何、方向等）。
    pub fn from_toml(text: &str) -> Result<Self, ConfigError> {
        let config: Self = toml::from_str(text).map_err(|e| ConfigError::Parse(e.to_string()))?;
        config.validate()?;
        Ok(config)
    }

    /// 从文件读取并解析。
    ///
    /// # Errors
    ///
    /// 读取失败、解析失败或校验失败。
    pub fn load(path: &std::path::Path) -> Result<Self, ConfigError> {
        let text = std::fs::read_to_string(path)
            .map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display())))?;
        Self::from_toml(&text)
    }

    /// 序列化回 TOML 文本。
    ///
    /// 与 [`Config::from_toml`] 往返等价（`from_toml(to_toml_string(c)) == c`），
    /// 因此整定工具改写增益后可以直接落盘成守护进程能读的配置。
    ///
    /// # Errors
    ///
    /// 序列化失败（类型定义与 TOML 数据模型冲突时才会发生，属实现缺陷）。
    pub fn to_toml_string(&self) -> Result<String, ConfigError> {
        toml::to_string(self).map_err(|e| ConfigError::Parse(e.to_string()))
    }

    /// 把配置写到文件。
    ///
    /// # Errors
    ///
    /// 序列化失败或写入失败。
    pub fn save(&self, path: &std::path::Path) -> Result<(), ConfigError> {
        let text = self.to_toml_string()?;
        std::fs::write(path, text)
            .map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display())))
    }

    /// 按名字查找电机下标。
    #[must_use]
    pub fn motor_index(&self, name: &str) -> Option<usize> {
        self.motors.iter().position(|m| m.name == name)
    }

    /// 覆写第 `index` 台电机的速度环增益。
    ///
    /// # Errors
    ///
    /// 下标越界时返回 [`ConfigError::Invalid`]。
    pub fn set_motor_gains(
        &mut self,
        index: usize,
        gains: VelocityGains,
    ) -> Result<(), ConfigError> {
        let count = self.motors.len();
        let motor = self.motors.get_mut(index).ok_or_else(|| {
            ConfigError::Invalid(format!("motor index {index} out of 0..{count}"))
        })?;
        motor.set_velocity_gains(gains);
        Ok(())
    }

    /// 按轮名覆写速度环增益（整定工具按名字写回时用）。
    ///
    /// # Errors
    ///
    /// 名字不存在时返回 [`ConfigError::Invalid`]。
    pub fn set_motor_gains_by_name(
        &mut self,
        name: &str,
        gains: VelocityGains,
    ) -> Result<(), ConfigError> {
        let index = self
            .motor_index(name)
            .ok_or_else(|| ConfigError::Invalid(format!("no motor named '{name}'")))?;
        self.set_motor_gains(index, gains)
    }

    /// 语义校验。
    ///
    /// # Errors
    ///
    /// 任一约束不满足时返回 [`ConfigError::Invalid`]。
    pub fn validate(&self) -> Result<(), ConfigError> {
        if self.motors.len() != 4 {
            return Err(ConfigError::Invalid(format!(
                "mecanum chassis needs exactly 4 motors (FL, FR, RL, RR), got {}",
                self.motors.len()
            )));
        }
        let m = &self.mecanum;
        if !(m.half_length > 0.0 && m.half_width > 0.0 && m.wheel_radius > 0.0) {
            return Err(ConfigError::Invalid(format!("mecanum geometry must be positive: {m:?}")));
        }
        let c = &self.chassis;
        if c.control_period_ms == 0
            || c.command_timeout_ms == 0
            || c.feedback_timeout_ms == 0
            || c.motor_command_timeout_ms == 0
        {
            return Err(ConfigError::Invalid(
                "control/command/feedback/motor-command timeouts must be > 0 ms".into(),
            ));
        }
        if c.motor_command_timeout_ms < c.control_period_ms {
            return Err(ConfigError::Invalid(format!(
                "motor_command_timeout_ms ({}) must be >= control_period_ms ({})",
                c.motor_command_timeout_ms, c.control_period_ms
            )));
        }
        if !(c.max_vx > 0.0 && c.max_vy > 0.0 && c.max_omega > 0.0) {
            return Err(ConfigError::Invalid("speed limits must be > 0".into()));
        }
        for motor in &self.motors {
            let gear_ratio_valid = motor.gear_ratio > 0.0;
            if !gear_ratio_valid {
                return Err(ConfigError::Invalid(format!(
                    "motor {}: gear_ratio must be > 0",
                    motor.name
                )));
            }
        }
        // 每总线单协议族：底盘四轮必须同属 ESC 族（C620/C610）。
        let chassis_families: std::collections::HashSet<ProtocolFamily> =
            self.motors.iter().map(|m| ProtocolFamily::of_esc_model(m.model)).collect();
        if chassis_families.len() > 1 {
            return Err(ConfigError::Invalid(
                "chassis bus mixes protocol families; keep one family per CAN bus".into(),
            ));
        }
        // 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）。
        if let Some(function) = &self.can.function
            && function.interface == self.can.interface
        {
            return Err(ConfigError::Invalid(format!(
                "function bus interface must differ from chassis interface (both '{}')",
                self.can.interface
            )));
        }
        // 电调 ID、方向、限流的细粒度校验由 EscBus::spawn 复核。
        Ok(())
    }

    /// `--mock` 演示 / 测试默认配置（与 `config/chassis.toml.example` 同构，
    /// PID 与仿真器增益匹配）。
    #[must_use]
    pub fn mock_default() -> Self {
        let motor = |name: &str, esc_id: u8, direction: f32| MotorConfig {
            name: name.into(),
            esc_id,
            model: EscModelSerde::C620,
            direction,
            gear_ratio: 3591.0 / 187.0,
            current_limit: 12_000,
            kp: 8.0,
            ki: 2.0,
            kd: 0.0,
            feedforward: 16384.0 / 9254.0,
        };
        Self {
            can: CanConfig { interface: "vcan0".into(), function: None },
            chassis: ChassisConfig::default(),
            mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 },
            motors: vec![
                motor("front_left", 1, 1.0),
                motor("front_right", 2, -1.0),
                motor("rear_left", 3, 1.0),
                motor("rear_right", 4, -1.0),
            ],
        }
    }

    /// 转换为 `EscBus` 配置（顺序 FL、FR、RL、RR）。
    #[must_use]
    pub fn to_esc_configs(&self) -> Vec<EscConfig> {
        self.motors
            .iter()
            .map(|m| EscConfig {
                name: m.name.clone(),
                esc_id: m.esc_id,
                model: m.model.into(),
                gear_ratio: m.gear_ratio,
                direction: m.direction,
                current_limit: m.current_limit,
                kp: m.kp,
                ki: m.ki,
                kd: m.kd,
                feedforward: m.feedforward,
            })
            .collect()
    }

    /// 转换为 `EscBus` 运行参数。
    #[must_use]
    pub fn bus_options(&self) -> BusOptions {
        BusOptions {
            period: Duration::from_millis(self.chassis.control_period_ms),
            feedback_timeout: Duration::from_millis(self.chassis.feedback_timeout_ms),
            command_timeout: Duration::from_millis(self.chassis.motor_command_timeout_ms),
        }
    }

    /// 麦轮运动学模型。
    #[must_use]
    pub fn kinematics(&self) -> Mecanum {
        Mecanum::new(self.mecanum.half_length, self.mecanum.half_width, self.mecanum.wheel_radius)
    }
}

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

    const EXAMPLE: &str = r#"
[can]
interface = "can0"

[chassis]
control_period_ms = 1
command_timeout_ms = 200
feedback_timeout_ms = 100

[mecanum]
half_length = 0.18
half_width = 0.20
wheel_radius = 0.0762

[[motor]]
name = "front_left"
esc_id = 1
direction = 1
gear_ratio = 19.203
current_limit = 12000
kp = 18.0
ki = 0.6
kd = 0.0
feedforward = 9.0

[[motor]]
name = "front_right"
esc_id = 2
direction = -1
gear_ratio = 19.203
current_limit = 12000
kp = 18.0
ki = 0.6
feedforward = 9.0

[[motor]]
name = "rear_left"
esc_id = 3
direction = 1
gear_ratio = 19.203
current_limit = 12000
kp = 18.0
ki = 0.6
feedforward = 9.0

[[motor]]
name = "rear_right"
esc_id = 4
direction = -1
gear_ratio = 19.203
current_limit = 12000
kp = 18.0
ki = 0.6
feedforward = 9.0
"#;

    #[test]
    fn parses_example() {
        let cfg = Config::from_toml(EXAMPLE).unwrap();
        assert_eq!(cfg.can.interface, "can0");
        assert_eq!(cfg.motors.len(), 4);
        assert_eq!(cfg.motors[0].name, "front_left");
        assert_eq!(cfg.motors[3].esc_id, 4);
        assert_eq!(cfg.motors[1].direction.to_bits(), (-1.0f32).to_bits());
        assert!(cfg.validate().is_ok());
        assert_eq!(cfg.bus_options().period, Duration::from_millis(1));
        assert_eq!(cfg.to_esc_configs().len(), 4);
    }

    #[test]
    fn minimal_config_uses_defaults() {
        let cfg = Config::from_toml(
            r#"
[can]
interface = "can1"
[mecanum]
half_length = 0.2
half_width = 0.25
wheel_radius = 0.0762
[[motor]]
name = "fl"
esc_id = 1
gear_ratio = 19.2
[[motor]]
name = "fr"
esc_id = 2
gear_ratio = 19.2
[[motor]]
name = "rl"
esc_id = 3
gear_ratio = 19.2
[[motor]]
name = "rr"
esc_id = 4
gear_ratio = 19.2
"#,
        )
        .unwrap();
        assert_eq!(cfg.chassis, ChassisConfig::default());
        assert_eq!(cfg.motors[0].direction.to_bits(), 1.0f32.to_bits());
        assert_eq!(cfg.motors[0].current_limit, 12_000);
        assert_eq!(cfg.motors[0].model, EscModelSerde::C620);
    }

    #[test]
    fn rejects_wrong_motor_count() {
        let mut cfg = Config::from_toml(EXAMPLE).unwrap();
        cfg.motors.pop();
        assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
    }

    #[test]
    fn rejects_bad_geometry() {
        let mut cfg = Config::from_toml(EXAMPLE).unwrap();
        cfg.mecanum.wheel_radius = 0.0;
        assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
    }

    #[test]
    fn rejects_unknown_fields() {
        let text = EXAMPLE.replace("[can]", "[can]\nstray = 1");
        assert!(matches!(Config::from_toml(&text), Err(ConfigError::Parse(_))));
    }

    #[test]
    fn mock_default_is_valid_and_wired_like_example() {
        let cfg = Config::mock_default();
        assert!(cfg.validate().is_ok());
        assert_eq!(cfg.motors.len(), 4);
        let esc: Vec<u8> = cfg.to_esc_configs().iter().map(|m| m.esc_id).collect();
        assert_eq!(esc, [1, 2, 3, 4]);
    }

    #[test]
    fn function_bus_must_differ_from_chassis_bus() {
        let mut cfg = Config::mock_default();
        cfg.can.function = Some(FunctionBusConfig { interface: "vcan0".into() });
        assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));

        cfg.can.function = Some(FunctionBusConfig { interface: "vcan1".into() });
        assert!(cfg.validate().is_ok());
    }

    #[test]
    fn rejects_mixed_protocol_families_on_chassis_bus() {
        let mut cfg = Config::mock_default();
        // 当前 ESC 族包含 C620 与 C610；若引入 GM6020 到底盘总线必须拒绝。
        // 这里验证家族集合逻辑：全部 C620/C610 仍为单族。
        cfg.motors[0].model = EscModelSerde::C610;
        assert!(cfg.validate().is_ok());

        // 人为构造跨族：通过 of_esc_model 映射覆盖后仍应单族（C610→Esc）。
        assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C620), ProtocolFamily::Esc);
        assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C610), ProtocolFamily::Esc);
    }

    #[test]
    fn parses_function_bus_seam() {
        let text = EXAMPLE.replace(
            "[can]\ninterface = \"can0\"",
            "[can]\ninterface = \"can0\"\n\n[can.function]\ninterface = \"can1\"",
        );
        let cfg = Config::from_toml(&text).unwrap();
        let function = cfg.can.function.as_ref().unwrap();
        assert_eq!(function.interface, "can1");
    }

    #[test]
    fn toml_round_trip_is_lossless() {
        let original = Config::from_toml(EXAMPLE).unwrap();
        let text = original.to_toml_string().unwrap();
        let reparsed = Config::from_toml(&text).unwrap();
        assert_eq!(reparsed, original);
        // 落盘文本必须仍然是合法配置（守护进程直接读它）。
        assert!(text.contains("[[motor]]"), "{text}");
        assert!(text.contains("interface = \"can0\""), "{text}");
    }

    #[test]
    fn round_trip_preserves_function_bus_and_timeouts() {
        let mut original = Config::mock_default();
        original.can.function = Some(FunctionBusConfig { interface: "can1".into() });
        original.chassis.motor_command_timeout_ms = 37;
        let reparsed = Config::from_toml(&original.to_toml_string().unwrap()).unwrap();
        assert_eq!(reparsed, original);
        assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37);
        assert_eq!(reparsed.can.function.unwrap().interface, "can1");
    }

    #[test]
    fn motor_gains_can_be_read_and_written_by_name() {
        let mut cfg = Config::from_toml(EXAMPLE).unwrap();
        assert_eq!(cfg.motor_index("rear_left"), Some(2));
        assert_eq!(cfg.motor_index("nope"), None);
        assert_eq!(cfg.motors[2].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0));

        let tuned = VelocityGains::new(2.0, 10.0, 0.1, 0.0);
        cfg.set_motor_gains_by_name("rear_left", tuned).unwrap();
        assert_eq!(cfg.motors[2].velocity_gains(), tuned);
        // 其余电机不受影响。
        assert_eq!(cfg.motors[0].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0));
        // 机械 / 安全字段保持不变。
        assert_eq!(cfg.motors[2].current_limit, 12_000);
        assert!((cfg.motors[2].gear_ratio - 19.203).abs() < 1e-6);
        assert!(cfg.validate().is_ok());
    }

    #[test]
    fn motor_gains_write_back_survives_round_trip() {
        let mut cfg = Config::mock_default();
        let tuned = VelocityGains::new(3.5, 22.0, 0.25, 0.75);
        cfg.set_motor_gains(0, tuned).unwrap();
        let reparsed = Config::from_toml(&cfg.to_toml_string().unwrap()).unwrap();
        assert_eq!(reparsed.motors[0].velocity_gains(), tuned);
    }

    #[test]
    fn motor_gains_out_of_range_is_rejected() {
        let mut cfg = Config::mock_default();
        assert!(matches!(
            cfg.set_motor_gains(9, VelocityGains::ZERO),
            Err(ConfigError::Invalid(_))
        ));
        assert!(matches!(
            cfg.set_motor_gains_by_name("ghost", VelocityGains::ZERO),
            Err(ConfigError::Invalid(_))
        ));
    }

    #[test]
    fn save_and_load_file_round_trip() {
        let dir = std::env::temp_dir().join("chassis-config-roundtrip");
        std::fs::create_dir_all(&dir).unwrap();
        let path = dir.join("chassis.toml");
        let original = Config::mock_default();
        original.save(&path).unwrap();
        let loaded = Config::load(&path).unwrap();
        assert_eq!(loaded, original);
        std::fs::remove_file(&path).unwrap();
    }
}