//! DJI 电调（C620 / C610）CAN 协议编解码。
//!
//! 帧格式（大端）：
//!
//! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）：
//!   `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令，
//!   槽位按组内电调顺序。
//! - 反馈（`0x200 + 电调ID`）：
//!   `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。
//!
//! 本模块只做纯编解码；主机侧速度/位置环由 [`motor_core`] 算法在
//! [`crate::bus`] 中按节拍调用。

use crate::frame::{
    COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE,
};
use thiserror::Error;

/// 电调 ID 下限。
pub const MIN_ESC_ID: u8 = 1;
/// 电调 ID 上限。
pub const MAX_ESC_ID: u8 = 8;

/// 电调型号决定电流标度。
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum EscModel {
    /// C620（配 GM3508）：±16384 ↔ ±20 A。
    C620,
    /// C610（配 M2006）：±10000 ↔ ±10 A。
    ///
    /// 接入前需实测核对。
    C610,
}

impl EscModel {
    /// 满量程电流命令（raw）。
    #[must_use]
    pub const fn max_current_raw(self) -> i16 {
        match self {
            Self::C620 => 16384,
            Self::C610 => 10_000,
        }
    }

    /// 满量程电流（安培）。
    #[must_use]
    pub const fn max_current_amp(self) -> f32 {
        match self {
            Self::C620 => 20.0,
            Self::C610 => 10.0,
        }
    }

    /// 安培 → raw（限幅）。
    #[must_use]
    pub fn raw_from_amp(self, amp: f32) -> i16 {
        let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp();
        (raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw())) as i16)
            .clamp(-self.max_current_raw(), self.max_current_raw())
    }

    /// raw → 安培。
    #[must_use]
    pub fn amp_from_raw(self, raw: i16) -> f32 {
        f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw())
    }
}

/// 一帧电调反馈（原始、电调侧，未经方向修正）。
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct EscFeedback {
    /// 转子机械角原始值（0..8191，对应 0..360°）。
    pub angle_raw: u16,
    /// 转子转速（rpm，电调侧符号）。
    pub speed_rpm: i16,
    /// 转矩电流 raw（电调侧符号）。
    pub current_raw: i16,
    /// 电机温度（℃）。
    pub temperature_c: i8,
}

/// 帧解码错误。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
pub enum DecodeError {
    /// 帧不属于电调反馈范围（0x201..=0x208）。
    #[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")]
    NotFeedback {
        /// 收到的帧 ID。
        id: u32,
    },
}

/// 电调 ID → 其电流命令所在的组控制帧 ID。
///
/// 返回 `None` 表示 ID 越界。
#[must_use]
pub const fn command_frame_id(esc_id: u8) -> Option<u32> {
    match esc_id {
        1..=4 => Some(COMMAND_FRAME_ID_PRIMARY),
        5..=8 => Some(COMMAND_FRAME_ID_SECONDARY),
        _ => None,
    }
}

/// 将 raw 电流限幅到电调满量程。
#[must_use]
pub const fn clamp_current(model: EscModel, raw: i16) -> i16 {
    if raw > model.max_current_raw() {
        model.max_current_raw()
    } else if raw < -model.max_current_raw() {
        -model.max_current_raw()
    } else {
        raw
    }
}

/// 组装一个控制组帧（`0x200` 或 `0x1FF`）。
///
/// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。
/// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成
/// （见 [`clamp_current`]）。
#[must_use]
pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame {
    let mut data = [0u8; 8];
    for (slot, raw) in currents.iter().enumerate() {
        data[slot * 2] = (raw >> 8) as u8;
        data[slot * 2 + 1] = (raw & 0xFF) as u8;
    }
    CanFrame::new(frame_id, data)
}

/// 组装一帧电调反馈（仿真器 / 测试用）。
#[must_use]
pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame {
    let mut data = [0u8; 8];
    data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes());
    data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes());
    data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes());
    data[6] = feedback.temperature_c as u8;
    CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data)
}

/// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。
///
/// # Errors
///
/// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]。
pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> {
    if frame.id <= FEEDBACK_FRAME_ID_BASE
        || frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID)
    {
        return Err(DecodeError::NotFeedback { id: frame.id });
    }
    let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8;
    Ok((
        esc_id,
        EscFeedback {
            angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]),
            speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]),
            current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]),
            temperature_c: frame.data[6] as i8,
        },
    ))
}

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

    #[test]
    fn command_frame_id_routing() {
        assert_eq!(command_frame_id(1), Some(0x200));
        assert_eq!(command_frame_id(4), Some(0x200));
        assert_eq!(command_frame_id(5), Some(0x1FF));
        assert_eq!(command_frame_id(8), Some(0x1FF));
        assert_eq!(command_frame_id(0), None);
        assert_eq!(command_frame_id(9), None);
    }

    #[test]
    fn encode_current_frame_bytes() {
        let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]);
        assert_eq!(frame.id, 0x200);
        assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]);

        let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]);
        assert_eq!(frame.id, 0x1FF);
        assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]);
    }

    #[test]
    fn decode_feedback_known_bytes() {
        let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
        let (esc_id, fb) = decode_feedback(&frame).unwrap();
        assert_eq!(esc_id, 5);
        assert_eq!(fb.angle_raw, 8191);
        assert_eq!(fb.speed_rpm, -364);
        assert_eq!(fb.current_raw, 123);
        assert_eq!(fb.temperature_c, 45);
    }

    #[test]
    fn decode_feedback_rejects_non_feedback_ids() {
        for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] {
            let frame = CanFrame::new(id, [0; 8]);
            assert_eq!(
                decode_feedback(&frame),
                Err(DecodeError::NotFeedback { id }),
                "id = 0x{id:03X}"
            );
        }
    }

    #[test]
    fn feedback_roundtrip_all_ids() {
        let fb = EscFeedback {
            angle_raw: 8191,
            speed_rpm: -9254,
            current_raw: -16384,
            temperature_c: -40,
        };
        for esc_id in MIN_ESC_ID..=MAX_ESC_ID {
            let frame = encode_feedback_frame(esc_id, &fb);
            assert_eq!(frame.id, 0x200 + u32::from(esc_id));
            assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb));
        }
    }

    #[test]
    fn amp_conversion() {
        let c620 = EscModel::C620;
        assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4);
        assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4);
        assert_eq!(c620.raw_from_amp(20.0), 16384);
        assert_eq!(c620.raw_from_amp(-5.0), -4096);
        // 超量程限幅。
        assert_eq!(c620.raw_from_amp(99.0), 16384);
        let c610 = EscModel::C610;
        assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4);
        assert_eq!(c610.raw_from_amp(10.0), 10000);
    }

    #[test]
    fn clamp_current_respects_model() {
        assert_eq!(clamp_current(EscModel::C620, 20000), 16384);
        assert_eq!(clamp_current(EscModel::C610, 15000), 10000);
        assert_eq!(clamp_current(EscModel::C620, 1000), 1000);
    }
}