darra-ethercat-master 2.3.0

//! SoE (Servo over EtherCAT) 完整实现
//!
//! 对齐 C# SoEInstance，提供:
//! - SoEInstance 结构体 (实例化方式)
//! - 原始字节读写 + 类型化读写 (i16/i32/u16/u32/f32/f64/string)
//! - 参数名称/单位/属性/最小值/最大值读取
//! - 程序命令执行 (SERCOS Procedure Command)
//! - IDN 列表/参数信息/AT/MDT 映射
//! - 通知监控 (启用/禁用/轮询)
//! - SoEParameter/SoEAttributes/SoEDataType 结构体
//!
//! 元素标志 (element_flags) 按 ETG.1000.6:
//!   0x01 = Data State, 0x02 = Name, 0x04 = Attribute, 0x08 = Unit,
//!   0x10 = Min, 0x20 = Max, 0x40 = Value, 0x80 = Default
//!
//! # 异步使用 (对齐 C# SoEInstance.ReadAsync / WriteAsync)
//!
//! 本 crate 提供**双轨**异步 API:
//!
//! ## 轨道 1: std::thread (无依赖, 默认启用)
//!
//! ```ignore
//! // 注: SoEInstance::new 为 pub(crate), 外部请用 Slave::soe() 获取后再转为关联函数
//! let handle = SoEInstance::read_blocking(master, slave, drive, 0x0040, 0x40, 500);
//! let data = handle.join().unwrap()?;
//! ```
//!
//! ## 轨道 2: tokio async (可选 feature `async-tokio`)
//!
//! ```ignore
//! let soe = slave.soe(drive_number);
//! let data = soe.read_async(0x0040, 0x40, 500).await?;
//! soe.write_async(0x0024, value_bytes, 0x40, 500).await?;
//! soe.execute_command_async(0x0127, 5000, 50).await?;
//! ```
//!
//! ## 超时与取消
//!
//! 使用 runtime 的 timeout 原语。DLL 调用无法中断, 超时只让 await 返回。
//! SoE 的 `timeout_ms` 参数是 DLL 内部的邮箱等待超时, 建议 await 侧
//! 至少留 1.5x 余量 (`tokio::time::timeout(1.5x * timeout_ms, ...)`)。

use crate::data::error::{DarraError, Result, SoEErrorCode};
use crate::utils::ffi;
use std::collections::HashMap;
use std::os::raw::c_int;
use std::sync::{Arc, Mutex};
use std::time::SystemTime;

/// SoE 元素标志：数据状态
pub const SOE_ELEM_DATA_STATE: u8 = 0x01;
/// SoE 元素标志：名称
pub const SOE_ELEM_NAME: u8 = 0x02;
/// SoE 元素标志：属性
pub const SOE_ELEM_ATTRIBUTE: u8 = 0x04;
/// SoE 元素标志：单位
pub const SOE_ELEM_UNIT: u8 = 0x08;
/// SoE 元素标志：最小值
pub const SOE_ELEM_MIN: u8 = 0x10;
/// SoE 元素标志：最大值
pub const SOE_ELEM_MAX: u8 = 0x20;
/// SoE 元素标志：当前值
pub const SOE_ELEM_VALUE: u8 = 0x40;
/// SoE 元素标志：默认值
pub const SOE_ELEM_DEFAULT: u8 = 0x80;

// ===================== SoE Element Flags (对齐 C# enum SoEElementFlags) =====================

/// SoE Element 位标志 (ETG.1000.6 §5.11 SoEHeader, 7 位位掩码).
///
/// 对齐 C# `SoEElementFlags`. 使用关联常量 + 位运算, 多个位可用 `|` 组合.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(transparent)]
pub struct SoEElementFlags(pub u8);

impl SoEElementFlags {
    pub const NONE:        Self = Self(0x00);
    pub const DATA_STATUS: Self = Self(0x01); // 数据状态字 (valid/invalid/busy)
    pub const NAME:        Self = Self(0x02); // IDN 名称
    pub const ATTRIBUTE:   Self = Self(0x04); // 属性 (类型/长度等)
    pub const UNIT:        Self = Self(0x08); // 单位字符串
    pub const MIN:         Self = Self(0x10); // 最小值
    pub const MAX:         Self = Self(0x20); // 最大值
    pub const VALUE:       Self = Self(0x40); // 操作数据值 (最常用)
    pub const DEFAULT:     Self = Self(0x80); // 默认值

    /// 内部位 -> 原始 u8 (供 DLL 调用)
    pub const fn bits(self) -> u8 { self.0 }

    /// 是否包含指定位
    pub const fn contains(self, other: Self) -> bool {
        (self.0 & other.0) == other.0
    }
}

impl std::ops::BitOr for SoEElementFlags {
    type Output = Self;
    fn bitor(self, rhs: Self) -> Self { Self(self.0 | rhs.0) }
}

impl std::ops::BitAnd for SoEElementFlags {
    type Output = Self;
    fn bitand(self, rhs: Self) -> Self { Self(self.0 & rhs.0) }
}

impl From<u8> for SoEElementFlags {
    fn from(v: u8) -> Self { Self(v) }
}

impl From<SoEElementFlags> for u8 {
    fn from(f: SoEElementFlags) -> u8 { f.0 }
}

// ===================== SERCOS IDN 编/解码 (对齐 DLL utils.c:SoEIdnEncode/Decode) =====================

/// 将 (Standard?, Set, Block) 三元组编码为 16 位 IDN.
///
/// 与 DLL `SoEIdnEncode` (utils.c:1438) 输出完全一致.
/// bit15 = 0 (标准) / 1 (厂商); bit14..12 = set; bit11..0 = block.
pub fn encode_idn(is_standard: bool, parameter_set: u8, data_block: u16) -> u16 {
    let mut idn: u16 = 0;
    if !is_standard {
        idn |= 0x8000;
    }
    idn |= ((parameter_set as u16) & 0x07) << 12;
    idn |= data_block & 0x0FFF;
    idn
}

/// 将 16 位 IDN 解码为 (is_standard, parameter_set, data_block) 三元组.
///
/// 与 DLL `SoEIdnDecode` (utils.c:1448) 输出完全一致.
pub fn decode_idn(idn: u16) -> (bool, u8, u16) {
    let is_standard = (idn & 0x8000) == 0;
    let parameter_set = ((idn >> 12) & 0x07) as u8;
    let data_block = idn & 0x0FFF;
    (is_standard, parameter_set, data_block)
}

/// 解析 SERCOS IDN 字符串 ("S-0-0040" / "P-1-0123") 为 16 位 IDN, 失败返回 `None`.
pub fn try_parse_sercos_idn(text: &str) -> Option<u16> {
    let parts: Vec<&str> = text.trim().split('-').collect();
    if parts.len() != 3 {
        return None;
    }
    let is_standard = match parts[0].to_ascii_uppercase().as_str() {
        "S" => true,
        "P" => false,
        _ => return None,
    };
    let set: u8 = parts[1].parse().ok()?;
    let block: u16 = parts[2].parse().ok()?;
    if set > 7 || block > 0x0FFF {
        return None;
    }
    Some(encode_idn(is_standard, set, block))
}

/// 格式化 16 位 IDN 为 "S-x-xxxx" / "P-x-xxxx" 字符串.
pub fn format_sercos_idn(idn: u16) -> String {
    let (is_standard, set, block) = decode_idn(idn);
    let prefix = if is_standard { "S" } else { "P" };
    format!("{}-{}-{:04}", prefix, set, block)
}

// ===================== SoE 数据类型 =====================

/// SoE 数据类型枚举
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(u8)]
pub enum SoEDataType {
    /// 二进制
    Binary = 0,
    /// 无符号整数
    UInt = 1,
    /// 有符号整数
    Int = 2,
    /// 十六进制
    Hexadecimal = 3,
    /// 字符串
    String = 4,
    /// IDN 引用
    IDN = 5,
    /// 浮点数
    Float = 6,
    /// 参数
    Parameter = 7,
}

impl From<u8> for SoEDataType {
    fn from(v: u8) -> Self {
        match v {
            0 => Self::Binary,
            1 => Self::UInt,
            2 => Self::Int,
            3 => Self::Hexadecimal,
            4 => Self::String,
            5 => Self::IDN,
            6 => Self::Float,
            7 => Self::Parameter,
            _ => Self::Binary,
        }
    }
}

// ===================== SoE 属性结构体 =====================

/// SoE 属性解码结果
#[derive(Debug, Clone)]
pub struct SoEAttributes {
    /// 评价因子
    pub evaluation_factor: u16,
    /// 数据长度指数 (实际字节 = 1 << length)
    pub length: u8,
    /// 是否为列表类型
    pub is_list: bool,
    /// 是否为命令
    pub is_command: bool,
    /// 数据类型
    pub data_type: SoEDataType,
    /// 小数位数
    pub decimals: u8,
    /// PreOp 状态下写保护
    pub write_protected_preop: bool,
    /// SafeOp 状态下写保护
    pub write_protected_safeop: bool,
    /// Op 状态下写保护
    pub write_protected_op: bool,
}

impl SoEAttributes {
    /// 从 u32 属性位域解码
    fn decode(attrs: u32) -> Self {
        Self {
            evaluation_factor: (attrs & 0xFFFF) as u16,
            length: ((attrs >> 16) & 0x3) as u8,
            is_list: ((attrs >> 18) & 0x1) == 1,
            is_command: ((attrs >> 19) & 0x1) == 1,
            data_type: SoEDataType::from(((attrs >> 20) & 0x7) as u8),
            decimals: ((attrs >> 24) & 0xF) as u8,
            write_protected_preop: ((attrs >> 28) & 0x1) == 1,
            write_protected_safeop: ((attrs >> 29) & 0x1) == 1,
            write_protected_op: ((attrs >> 30) & 0x1) == 1,
        }
    }

    /// 是否只读
    pub fn is_read_only(&self) -> bool {
        self.write_protected_preop && self.write_protected_safeop && self.write_protected_op
    }

    /// 获取访问权限字符串 (对齐 C# AccessMode)
    ///
    /// 返回 "RO" (完全只读), "RW" (完全读写), "RW*" (部分状态可写)
    pub fn access_mode(&self) -> &'static str {
        if self.is_read_only() {
            "RO"
        } else if !self.write_protected_preop && !self.write_protected_safeop && !self.write_protected_op {
            "RW"
        } else {
            "RW*"
        }
    }
}

// ===================== SoE 参数信息 =====================

/// SoE IDN 参数完整信息
#[derive(Debug, Clone)]
pub struct SoEParameter {
    /// IDN 编号
    pub idn: u16,
    /// 参数名称
    pub name: String,
    /// 参数单位
    pub unit: String,
    /// 属性
    pub attributes: SoEAttributes,
    /// 当前值 (原始字节)
    pub value: Vec<u8>,
    /// 默认值
    pub default_value: Vec<u8>,
    /// 最小值
    pub min_value: Vec<u8>,
    /// 最大值
    pub max_value: Vec<u8>,
}

impl SoEParameter {
    /// 获取 SERCOS IDN 格式字符串 (S-x-xxxx 或 P-x-xxxx)
    pub fn sercos_idn(&self) -> String {
        let prefix = if self.idn < 0x8000 { "S" } else { "P" };
        let type_num = (self.idn >> 12) & 0x7;
        let number = self.idn & 0x0FFF;
        format!("{}-{}-{:04}", prefix, type_num, number)
    }

    /// IDN 类别
    pub fn category(&self) -> &'static str {
        if self.idn < 0x8000 { "Standard" }
        else if self.idn >= 0xC000 { "Vendor" }
        else { "Product" }
    }

    /// 是否只读
    pub fn is_read_only(&self) -> bool {
        self.attributes.is_read_only()
    }

    /// 获取访问权限字符串 (对齐 C# AccessMode)
    pub fn access_mode(&self) -> &'static str {
        self.attributes.access_mode()
    }
}

impl std::fmt::Display for SoEParameter {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "{} {}", self.sercos_idn(), self.name)
    }
}

// ===================== SoE 映射信息 =====================

/// SoE IDN 映射条目
#[derive(Debug, Clone)]
pub struct ServoMappingEntry {
    /// IDN 编号
    pub idn: u16,
    /// 参数名称
    pub name: String,
    /// 数据长度 (位)
    pub bit_length: i32,
}

impl ServoMappingEntry {
    /// 数据长度 (字节)
    pub fn byte_length(&self) -> i32 {
        (self.bit_length + 7) / 8
    }
}

/// SoE AT/MDT 映射信息
#[derive(Debug, Clone)]
pub struct ServoMappingInfo {
    /// AT (输入) 映射列表
    pub at_mapping: Vec<ServoMappingEntry>,
    /// MDT (输出) 映射列表
    pub mdt_mapping: Vec<ServoMappingEntry>,
    /// AT 总位数
    pub at_bit_size: i32,
    /// MDT 总位数
    pub mdt_bit_size: i32,
}

impl ServoMappingInfo {
    /// AT 总字节数
    pub fn at_byte_size(&self) -> i32 { (self.at_bit_size + 7) / 8 }
    /// MDT 总字节数
    pub fn mdt_byte_size(&self) -> i32 { (self.mdt_bit_size + 7) / 8 }
}

// ===================== SoE 通知事件 =====================

/// SoE 通知事件参数
#[derive(Debug, Clone)]
pub struct SoENotificationEventArgs {
    /// 从站索引
    pub slave_index: u16,
    /// 驱动器编号
    pub drive_number: u8,
    /// 发生变化的 IDN
    pub idn: u16,
    /// 新值
    pub new_value: Option<Vec<u8>>,
    /// 旧值
    pub old_value: Option<Vec<u8>>,
}

impl SoENotificationEventArgs {
    /// 获取 SERCOS IDN 格式字符串
    pub fn sercos_idn(&self) -> String {
        let prefix = if self.idn < 0x8000 { "S" } else { "P" };
        let type_num = (self.idn >> 12) & 0x7;
        let number = self.idn & 0x0FFF;
        format!("{}-{}-{:04}", prefix, type_num, number)
    }
}

// ===================== SoE Notification / Emergency 回调 (对齐 C# SoEInstance.Add*Callback) =====================

/// SoE 通知事件 (对齐 C# `SoEInstance.NotificationEvent`).
///
/// 实时语义: 由用户回调注册后, DLL 监控到 IDN 变化 / 推送 Notification 服务时触发.
#[derive(Debug, Clone)]
pub struct SoENotificationEvent {
    /// 主站索引
    pub master_index: u16,
    /// 从站索引
    pub slave_index: u16,
    /// 驱动器编号
    pub drive_number: u8,
    /// 发生变化的 IDN
    pub idn: u16,
    /// 新值 (Element Value)
    pub new_value: Option<Vec<u8>>,
    /// 旧值 (上次 poll 读到的)
    pub old_value: Option<Vec<u8>>,
    /// 触发时刻
    pub timestamp: SystemTime,
}

impl SoENotificationEvent {
    /// SERCOS IDN 格式 "S-0-0016" / "P-1-0100"
    pub fn sercos_idn(&self) -> String {
        let prefix = if self.idn < 0x8000 { "S" } else { "P" };
        let type_num = (self.idn >> 12) & 0x7;
        let number = self.idn & 0x0FFF;
        format!("{}-{}-{:04}", prefix, type_num, number)
    }
}

/// SoE Emergency 事件 (对齐 C# `SoEInstance.EmergencyEvent`, ETG.5003).
#[derive(Debug, Clone)]
pub struct SoEEmergencyEvent {
    /// 主站索引
    pub master_index: u16,
    /// 从站索引
    pub slave_index: u16,
    /// 驱动器编号
    pub drive_number: u8,
    /// IDN (Emergency 关联的参数)
    pub idn: u16,
    /// SoE 错误码 (ETG.5003)
    pub error_code: SoEErrorCode,
    /// Emergency 附加数据 (原始字节)
    pub data: Vec<u8>,
    /// 触发时刻
    pub timestamp: SystemTime,
}

/// SoE 通知回调类型别名 (对齐 Z-4 FSoE 分发器模式).
///
/// 使用 `Arc<dyn Fn(...) + Send + Sync>` 支持多订阅者共享且线程安全.
pub type SoENotificationCallback = Arc<dyn Fn(&SoENotificationEvent) + Send + Sync>;

/// SoE Emergency 回调类型别名.
pub type SoEEmergencyCallback = Arc<dyn Fn(&SoEEmergencyEvent) + Send + Sync>;

// ===================== SoE 标准 IDN 常量 =====================

/// SoE 标准 IDN 常量
pub mod standard_idn {
    /// IDN 列表 (S-0-0000)
    pub const IDN_LIST: u16 = 0x0000;
    /// AT 配置 - 输入映射 (S-0-0016)
    pub const AT_CONFIG: u16 = 0x0010;
    /// MDT 配置 - 输出映射 (S-0-0024)
    pub const MDT_CONFIG: u16 = 0x0018;
    /// 通信周期时间 (S-0-0001)
    pub const CYCLE_TIME: u16 = 0x0001;
    /// 通知请求 (S-0-0127)
    pub const NOTIFICATION_REQUEST: u16 = 0x007F;
}

// ===================== SoEInstance 实例 =====================

/// SoE 实例 - 对齐 C# SoEInstance
///
/// 为单个从站/驱动器提供完整的 SoE 参数读写功能。
/// 通过 `Slave::soe()` 获取。
pub struct SoEInstance {
    master_index: u16,
    slave_index: u16,
    drive_number: u8,
    /// 已监控的 IDN 及其上次值 (用于轮询检测)
    monitored_idns: Mutex<HashMap<u16, Option<Vec<u8>>>>,
    /// 最近一次 SoE 错误码
    last_soe_error: Option<SoEErrorCode>,
    /// SoE 通知回调列表 (对齐 C# `event NotificationReceived`)
    notification_callbacks: Arc<Mutex<Vec<SoENotificationCallback>>>,
    /// SoE Emergency 回调列表 (对齐 C# `event EmergencyReceived`)
    emergency_callbacks: Arc<Mutex<Vec<SoEEmergencyCallback>>>,
}

impl SoEInstance {
    /// 创建 SoE 实例
    pub(crate) fn new(master_index: u16, slave_index: u16, drive_number: u8) -> Self {
        Self {
            master_index,
            slave_index,
            drive_number,
            monitored_idns: Mutex::new(HashMap::new()),
            last_soe_error: None,
            notification_callbacks: Arc::new(Mutex::new(Vec::new())),
            emergency_callbacks: Arc::new(Mutex::new(Vec::new())),
        }
    }

    /// 主站索引
    pub fn master_index(&self) -> u16 { self.master_index }

    /// 从站索引
    pub fn slave_index(&self) -> u16 { self.slave_index }

    /// 驱动器编号
    pub fn drive_number(&self) -> u8 { self.drive_number }

    /// 从站是否支持 SoE 邮箱协议 (对齐 C# SoEInstance.IsSupported).
    ///
    /// 读取 SII mbx_proto bit 4 (ECT_MBXPROT_SOE = 0x10) 判断支持情况.
    /// DLL 调用失败时保守返回 true (未知能力视为支持).
    pub fn is_supported(&self) -> bool {
        let proto = unsafe { ffi::GetSlaveMailboxProto(self.master_index, self.slave_index) };
        (proto & 0x10) != 0
    }

    /// 获取最近一次 SoE 错误码
    pub fn last_soe_error(&self) -> Option<SoEErrorCode> { self.last_soe_error }

    // ==================== Notification / Emergency 回调 ====================

    /// 注册 SoE 通知回调 (对齐 C# `SoEInstance.NotificationReceived` 事件 +=).
    ///
    /// 支持多订阅者; 同一 Arc 多次注册会被多次调用. 使用 `Arc::ptr_eq`
    /// 判等, 若需移除请保留 `Arc` 的克隆用于后续 `remove_notification_callback`.
    pub fn add_notification_callback(&self, cb: SoENotificationCallback) {
        if let Ok(mut list) = self.notification_callbacks.lock() {
            list.push(cb);
        }
    }

    /// 移除 SoE 通知回调 (按 `Arc::ptr_eq` 匹配).
    pub fn remove_notification_callback(&self, cb: &SoENotificationCallback) {
        if let Ok(mut list) = self.notification_callbacks.lock() {
            list.retain(|c| !Arc::ptr_eq(c, cb));
        }
    }

    /// 注册 SoE Emergency 回调.
    pub fn add_emergency_callback(&self, cb: SoEEmergencyCallback) {
        if let Ok(mut list) = self.emergency_callbacks.lock() {
            list.push(cb);
        }
    }

    /// 移除 SoE Emergency 回调 (按 `Arc::ptr_eq` 匹配).
    pub fn remove_emergency_callback(&self, cb: &SoEEmergencyCallback) {
        if let Ok(mut list) = self.emergency_callbacks.lock() {
            list.retain(|c| !Arc::ptr_eq(c, cb));
        }
    }

    /// 触发 Notification 事件 (内部使用: 由 `poll_notifications` 或外部桥接层调用).
    ///
    /// 快照回调列表后释放锁再调用, 避免回调中注册 / 注销时死锁.
    /// 用 `catch_unwind` 吞掉 panic, 防止影响调用方.
    pub fn fire_notification(&self, ev: &SoENotificationEvent) {
        let snapshot: Vec<SoENotificationCallback> = match self.notification_callbacks.lock() {
            Ok(list) => list.clone(),
            Err(_) => return,
        };
        for cb in &snapshot {
            let _ = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| cb(ev)));
        }
    }

    /// 触发 Emergency 事件 (内部使用).
    pub fn fire_emergency(&self, ev: &SoEEmergencyEvent) {
        let snapshot: Vec<SoEEmergencyCallback> = match self.emergency_callbacks.lock() {
            Ok(list) => list.clone(),
            Err(_) => return,
        };
        for cb in &snapshot {
            let _ = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| cb(ev)));
        }
    }

    // ==================== 原始字节读写 ====================

    /// 读取 IDN 参数 (原始字节)
    pub fn read(&self, idn: u16, element_flags: u8, timeout_ms: i32) -> Result<Vec<u8>> {
        soe_read_raw(self.master_index, self.slave_index, idn, self.drive_number, element_flags, timeout_ms * 1000)
    }

    /// 写入 IDN 参数 (原始字节)
    pub fn write(&self, idn: u16, data: &[u8], element_flags: u8, timeout_ms: i32) -> Result<()> {
        soe_write_raw(self.master_index, self.slave_index, idn, self.drive_number, element_flags, data, timeout_ms * 1000)
    }

    // ==================== 类型化读取 ====================

    /// 读取 i16 值
    pub fn read_i16(&self, idn: u16, timeout_ms: i32) -> Result<i16> {
        soe_read_i16(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)
    }

    /// 读取 i32 值
    pub fn read_i32(&self, idn: u16, timeout_ms: i32) -> Result<i32> {
        soe_read_i32(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)
    }

    /// 读取 u16 值
    pub fn read_u16(&self, idn: u16, timeout_ms: i32) -> Result<u16> {
        soe_read_u16(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)
    }

    /// 读取 u32 值
    pub fn read_u32(&self, idn: u16, timeout_ms: i32) -> Result<u32> {
        soe_read_u32(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)
    }

    /// 读取 f32 值
    pub fn read_f32(&self, idn: u16, timeout_ms: i32) -> Result<f32> {
        soe_read_f32(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)
    }

    /// 读取 f64 值
    pub fn read_f64(&self, idn: u16, timeout_ms: i32) -> Result<f64> {
        soe_read_f64(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)
    }

    /// 读取字符串值
    pub fn read_string(&self, idn: u16, timeout_ms: i32) -> Result<String> {
        soe_read_string(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)
    }

    // ==================== 类型化写入 ====================

    /// 写入 i16 值
    pub fn write_i16(&self, idn: u16, value: i16, timeout_ms: i32) -> Result<()> {
        soe_write_i16(self.master_index, self.slave_index, idn, self.drive_number, value, timeout_ms * 1000)
    }

    /// 写入 i32 值
    pub fn write_i32(&self, idn: u16, value: i32, timeout_ms: i32) -> Result<()> {
        soe_write_i32(self.master_index, self.slave_index, idn, self.drive_number, value, timeout_ms * 1000)
    }

    /// 写入 u16 值
    pub fn write_u16(&self, idn: u16, value: u16, timeout_ms: i32) -> Result<()> {
        soe_write_u16(self.master_index, self.slave_index, idn, self.drive_number, value, timeout_ms * 1000)
    }

    /// 写入 u32 值
    pub fn write_u32(&self, idn: u16, value: u32, timeout_ms: i32) -> Result<()> {
        soe_write_u32(self.master_index, self.slave_index, idn, self.drive_number, value, timeout_ms * 1000)
    }

    // ==================== 扩展读取 ====================

    /// 读取 IDN 参数名称 (Element 0x02)
    ///
    /// [2026-05-08 修复乱码] SoE IDN 名走多编码兜底 (UTF-8/ASCII/Latin-1).
    pub fn read_name(&self, idn: u16, timeout_ms: i32) -> Result<String> {
        let data = self.read(idn, SOE_ELEM_NAME, timeout_ms)?;
        let end = data.iter().position(|&b| b == 0).unwrap_or(data.len());
        Ok(crate::utils::help::decode_ethercat_string(&data[..end]))
    }

    /// 读取 IDN 参数单位 (Element 0x08)
    ///
    /// [2026-05-08 修复乱码] SoE IDN 单位走多编码兜底 (UTF-8/ASCII/Latin-1).
    pub fn read_unit(&self, idn: u16, timeout_ms: i32) -> Result<String> {
        let data = self.read(idn, SOE_ELEM_UNIT, timeout_ms)?;
        let end = data.iter().position(|&b| b == 0).unwrap_or(data.len());
        Ok(crate::utils::help::decode_ethercat_string(&data[..end]))
    }

    /// 读取 IDN 参数属性
    pub fn read_attributes(&self, idn: u16, timeout_ms: i32) -> Result<SoEAttributes> {
        let attrs = soe_read_attributes(self.master_index, self.slave_index, idn, self.drive_number, timeout_ms * 1000)?;
        Ok(SoEAttributes::decode(attrs))
    }

    /// 读取最小值 (Element 0x10)
    pub fn read_min_value(&self, idn: u16, timeout_ms: i32) -> Result<Vec<u8>> {
        self.read(idn, SOE_ELEM_MIN, timeout_ms)
    }

    /// 读取最大值 (Element 0x20)
    pub fn read_max_value(&self, idn: u16, timeout_ms: i32) -> Result<Vec<u8>> {
        self.read(idn, SOE_ELEM_MAX, timeout_ms)
    }

    /// 读取默认值 (Element 0x80)
    pub fn read_default_value(&self, idn: u16, timeout_ms: i32) -> Result<Vec<u8>> {
        self.read(idn, SOE_ELEM_DEFAULT, timeout_ms)
    }

    /// 读取 Data State (Element 0x01)
    pub fn read_data_state(&self, idn: u16, timeout_ms: i32) -> Result<u16> {
        let data = self.read(idn, SOE_ELEM_DATA_STATE, timeout_ms)?;
        if data.len() < 2 { return Err(DarraError::SoeFailed(idn)); }
        Ok(u16::from_le_bytes([data[0], data[1]]))
    }

    // ==================== 程序命令执行 ====================

    /// 执行 SoE 程序命令 (SERCOS Procedure Command)
    ///
    /// 写入 Data State bit 0 = 1 启动命令，轮询等待 bit 0 = 0 完成。
    pub fn execute_command(&self, idn: u16, timeout_ms: i32, poll_interval_ms: i32) -> Result<()> {
        // 读取当前 Data State
        let current = self.read_data_state(idn, timeout_ms)?;
        // 设置 bit 0 = 1, 清除 bit 1
        let start_value = (current & 0xFFFC) | 0x0001;
        self.write(idn, &start_value.to_le_bytes(), SOE_ELEM_DATA_STATE, timeout_ms)?;

        // 轮询等待
        let start = std::time::Instant::now();
        let timeout = std::time::Duration::from_millis(timeout_ms as u64);
        let interval = std::time::Duration::from_millis(poll_interval_ms as u64);

        while start.elapsed() < timeout {
            std::thread::sleep(interval);
            if let Ok(state) = self.read_data_state(idn, timeout_ms) {
                if (state & 0x0002) != 0 {
                    return Err(DarraError::SoeFailed(idn)); // bit 1 = 错误
                }
                if (state & 0x0001) == 0 {
                    return Ok(()); // bit 0 = 0, 完成
                }
            }
        }
        Err(DarraError::Timeout)
    }

    // ==================== IDN 列表/参数信息 ====================

    /// 获取支持的 IDN 列表 (对齐 C# GetAvailableIDNs)
    pub fn get_available_idns(&self, timeout_ms: i32) -> Result<Vec<u16>> {
        self.available_idns(timeout_ms)
    }

    /// 获取 IDN 完整参数信息 (对齐 C# GetParameterInfo)
    pub fn get_parameter_info(&self, idn: u16, timeout_ms: i32) -> SoEParameter {
        self.parameter_info(idn, timeout_ms)
    }

    /// 获取 AT/MDT 映射配置 (对齐 C# GetIDNMapping)
    pub fn get_idn_mapping(&self, timeout_ms: i32) -> ServoMappingInfo {
        self.idn_mapping(timeout_ms)
    }

    /// 获取正在监控的 IDN 列表 (对齐 C# GetMonitoredIDNs)
    pub fn get_monitored_idns(&self) -> Vec<u16> {
        self.monitored_idns()
    }

    /// 获取所有驱动器的 AT/MDT 映射 (对齐 C# GetAllDriveMappings)
    ///
    /// 遍历最多 8 个驱动器, 返回每个驱动器的映射信息
    pub fn get_all_drive_mappings(&self, timeout_ms: i32) -> HashMap<u8, ServoMappingInfo> {
        let mut result = HashMap::new();
        for drive_no in 0u8..8 {
            let temp = SoEInstance::new(self.master_index, self.slave_index, drive_no);
            let mapping = temp.idn_mapping(timeout_ms);
            if !mapping.at_mapping.is_empty() || !mapping.mdt_mapping.is_empty() {
                result.insert(drive_no, mapping);
            }
        }
        result
    }

    /// 读取单个 SoE 参数 (对齐 C# SoE manager ReadParameter)
    ///
    /// 读取 IDN 的名称、单位、属性和当前值
    pub fn read_parameter(&self, idn: u16, timeout_ms: i32) -> SoEParameter {
        self.parameter_info(idn, timeout_ms)
    }

    /// 批量读取常见 SoE 参数 (对齐 C# SoE manager ReadAllParameters)
    ///
    /// 读取标准 Sercos IDN 子集的参数信息, 跳过读取失败的条目
    pub fn read_all_parameters(&self, timeout_ms: i32) -> Vec<SoEParameter> {
        let standard_idns: &[u16] = &[
            0x0000, // IDN of all IDNs
            0x0001, // Control unit cycle time
            0x0002, // Position data scaling type
            0x0003, // Position data scaling factor
            0x0004, // Position data scaling exponent
            0x0010, // Drive status
            0x0011, // Position feedback value 1
            0x0012, // Position command value
            0x0013, // Velocity feedback value
            0x0014, // Velocity command value
            0x0015, // Torque/force feedback value
            0x0016, // Torque/force command value
            0x0021, // Positioning window
            0x0024, // Maximum velocity
            0x0032, // Drive control
            0x0033, // Position command value from interpolator
            0x0040, // Drive control word
            0x0047, // Drive status word
        ];

        let mut params = Vec::new();
        for &idn in standard_idns {
            // 尝试读取, 跳过失败的 IDN
            let param = self.parameter_info(idn, timeout_ms);
            // 只添加成功读取到值的参数
            if !param.name.is_empty() || !param.value.is_empty() {
                params.push(param);
            }
        }
        params
    }

    /// 写入 SoE 参数 (对齐 C# SoE manager WriteParameter)
    pub fn write_parameter(&self, idn: u16, data: &[u8], timeout_ms: i32) -> Result<()> {
        if data.is_empty() {
            return Err(DarraError::InvalidParameter("数据不能为空".to_string()));
        }
        self.write(idn, data, SOE_ELEM_VALUE, timeout_ms)
    }

    /// 获取支持的 IDN 列表 (内部实现)
    pub fn available_idns(&self, timeout_ms: i32) -> Result<Vec<u16>> {
        soe_list_idns(self.master_index, self.slave_index, self.drive_number, timeout_ms * 1000)
    }

    /// 获取 IDN 完整参数信息
    pub fn parameter_info(&self, idn: u16, timeout_ms: i32) -> SoEParameter {
        let mut param = SoEParameter {
            idn,
            name: String::new(),
            unit: String::new(),
            attributes: SoEAttributes::decode(0),
            value: Vec::new(),
            default_value: Vec::new(),
            min_value: Vec::new(),
            max_value: Vec::new(),
        };

        // 读取名称
        if let Ok(name) = self.read_name(idn, timeout_ms) {
            param.name = name;
        }
        // 读取单位
        if let Ok(unit) = self.read_unit(idn, timeout_ms) {
            param.unit = unit;
        }
        // 读取属性
        if let Ok(attrs) = self.read_attributes(idn, timeout_ms) {
            param.attributes = attrs;
        }
        // 读取当前值
        if let Ok(value) = self.read(idn, SOE_ELEM_VALUE, timeout_ms) {
            param.value = value;
        }
        // 读取最小/最大值
        if let Ok(min) = self.read_min_value(idn, timeout_ms) {
            param.min_value = min;
        }
        if let Ok(max) = self.read_max_value(idn, timeout_ms) {
            param.max_value = max;
        }
        // 读取默认值
        if let Ok(def) = self.read_default_value(idn, timeout_ms) {
            param.default_value = def;
        }

        param
    }

    // ==================== AT/MDT 映射 ====================

    /// 读取 AT/MDT 映射配置
    pub fn idn_mapping(&self, timeout_ms: i32) -> ServoMappingInfo {
        let timeout_us = timeout_ms * 1000;
        let mut mapping = ServoMappingInfo {
            at_mapping: Vec::new(),
            mdt_mapping: Vec::new(),
            at_bit_size: 16, // 状态字始终映射
            mdt_bit_size: 16, // 命令字始终映射
        };

        // 读取 AT 配置 (IDN 0x0010)
        if let Ok(at_data) = self.read(standard_idn::AT_CONFIG, SOE_ELEM_VALUE, timeout_ms) {
            self.parse_mapping(&at_data, &mut mapping.at_mapping, &mut mapping.at_bit_size, timeout_us);
        }

        // 读取 MDT 配置 (IDN 0x0018)
        if let Ok(mdt_data) = self.read(standard_idn::MDT_CONFIG, SOE_ELEM_VALUE, timeout_ms) {
            self.parse_mapping(&mdt_data, &mut mapping.mdt_mapping, &mut mapping.mdt_bit_size, timeout_us);
        }

        mapping
    }

    /// 解析映射数据
    fn parse_mapping(&self, data: &[u8], entries: &mut Vec<ServoMappingEntry>, total_bits: &mut i32, timeout_us: i32) {
        if data.len() < 4 { return; }
        let current_length = u16::from_le_bytes([data[0], data[1]]) as usize;
        let idn_count = current_length / 2;

        for i in 0..idn_count {
            let offset = 4 + i * 2;
            if offset + 1 >= data.len() { break; }
            let mapped_idn = u16::from_le_bytes([data[offset], data[offset + 1]]);

            let mut entry = ServoMappingEntry {
                idn: mapped_idn,
                name: format!("IDN 0x{:04X}", mapped_idn),
                bit_length: 0,
            };

            // 读取属性获取长度
            if let Ok(attrs) = soe_read_attributes(self.master_index, self.slave_index, mapped_idn, self.drive_number, timeout_us) {
                let decoded = SoEAttributes::decode(attrs);
                if !decoded.is_list {
                    entry.bit_length = 8 << decoded.length;
                    *total_bits += entry.bit_length;
                }
            }

            // 尝试读取名称
            if let Ok(name) = self.read_name(mapped_idn, 500) {
                entry.name = name;
            }

            entries.push(entry);
        }
    }

    // ==================== 通知管理 ====================

    /// 启用指定 IDN 的变化通知
    ///
    /// 将 IDN 加入轮询监控列表，并尝试通过 S-0-0127 启用硬件通知。
    pub fn enable_notification(&self, idn: u16, timeout_ms: i32) -> bool {
        // 尝试硬件通知
        let hardware = self.write(standard_idn::NOTIFICATION_REQUEST, &idn.to_le_bytes(), 0, timeout_ms).is_ok();

        // 读取当前值作为基线
        let current = self.read(idn, SOE_ELEM_VALUE, timeout_ms).ok();
        if let Ok(mut map) = self.monitored_idns.lock() {
            map.insert(idn, current);
        }

        hardware
    }

    /// 禁用指定 IDN 的变化通知
    pub fn disable_notification(&self, idn: u16) {
        if let Ok(mut map) = self.monitored_idns.lock() {
            map.remove(&idn);
        }
    }

    /// 禁用所有通知
    pub fn disable_all_notifications(&self) {
        if let Ok(mut map) = self.monitored_idns.lock() {
            map.clear();
        }
    }

    /// 获取正在监控的 IDN 列表
    pub fn monitored_idns(&self) -> Vec<u16> {
        self.monitored_idns.lock().map(|m| m.keys().copied().collect()).unwrap_or_default()
    }

    /// 轮询检测所有已监控 IDN 的变化
    ///
    /// 返回 (变化数量, 变化事件列表)
    pub fn poll_notifications(&self, timeout_ms: i32) -> Vec<SoENotificationEventArgs> {
        let idns: Vec<u16> = self.monitored_idns.lock()
            .map(|m| m.keys().copied().collect())
            .unwrap_or_default();

        let mut changes = Vec::new();
        for idn in idns {
            if let Ok(new_value) = self.read(idn, SOE_ELEM_VALUE, timeout_ms) {
                let old_value = self.monitored_idns.lock().ok()
                    .and_then(|m| m.get(&idn).cloned())
                    .flatten();

                let changed = match (&old_value, &Some(new_value.clone())) {
                    (None, Some(_)) => true,
                    (Some(a), Some(b)) => a != b,
                    _ => false,
                };

                if changed {
                    if let Ok(mut map) = self.monitored_idns.lock() {
                        map.insert(idn, Some(new_value.clone()));
                    }
                    // 通知回调 (若有注册)
                    let ev = SoENotificationEvent {
                        master_index: self.master_index,
                        slave_index: self.slave_index,
                        drive_number: self.drive_number,
                        idn,
                        new_value: Some(new_value.clone()),
                        old_value: old_value.clone(),
                        timestamp: SystemTime::now(),
                    };
                    self.fire_notification(&ev);

                    changes.push(SoENotificationEventArgs {
                        slave_index: self.slave_index,
                        drive_number: self.drive_number,
                        idn,
                        new_value: Some(new_value),
                        old_value,
                    });
                }
            }
        }
        changes
    }
}

// ===================== 模块级自由函数 (保持兼容) =====================

/// 读取 SoE IDN 的 i16 值
pub fn soe_read_i16(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<i16> {
    let data = soe_read_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, timeout_us)?;
    if data.len() < 2 { return Err(DarraError::SoeFailed(idn)); }
    Ok(i16::from_le_bytes([data[0], data[1]]))
}

/// 读取 SoE IDN 的 i32 值
pub fn soe_read_i32(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<i32> {
    let data = soe_read_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, timeout_us)?;
    if data.len() < 4 { return Err(DarraError::SoeFailed(idn)); }
    Ok(i32::from_le_bytes([data[0], data[1], data[2], data[3]]))
}

/// 读取 SoE IDN 的 u16 值
pub fn soe_read_u16(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<u16> {
    let data = soe_read_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, timeout_us)?;
    if data.len() < 2 { return Err(DarraError::SoeFailed(idn)); }
    Ok(u16::from_le_bytes([data[0], data[1]]))
}

/// 读取 SoE IDN 的 u32 值
pub fn soe_read_u32(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<u32> {
    let data = soe_read_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, timeout_us)?;
    if data.len() < 4 { return Err(DarraError::SoeFailed(idn)); }
    Ok(u32::from_le_bytes([data[0], data[1], data[2], data[3]]))
}

/// 读取 SoE IDN 的 f32 值
pub fn soe_read_f32(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<f32> {
    let bits = soe_read_u32(master_index, slave_index, idn, drive_no, timeout_us)?;
    Ok(f32::from_bits(bits))
}

/// 读取 SoE IDN 的 f64 值
pub fn soe_read_f64(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<f64> {
    let data = soe_read_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, timeout_us)?;
    if data.len() < 8 { return Err(DarraError::SoeFailed(idn)); }
    let bits = u64::from_le_bytes([
        data[0], data[1], data[2], data[3],
        data[4], data[5], data[6], data[7],
    ]);
    Ok(f64::from_bits(bits))
}

/// 读取 SoE IDN 的字符串值
///
/// [2026-05-08 修复乱码] SoE IDN 字符串值走多编码兜底 (UTF-8/ASCII/Latin-1).
pub fn soe_read_string(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<String> {
    let data = soe_read_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, timeout_us)?;
    let end = data.iter().position(|&b| b == 0).unwrap_or(data.len());
    Ok(crate::utils::help::decode_ethercat_string(&data[..end]))
}

/// 写入 SoE IDN 的 i16 值
pub fn soe_write_i16(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, value: i16, timeout_us: i32,
) -> Result<()> {
    soe_write_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, &value.to_le_bytes(), timeout_us)
}

/// 写入 SoE IDN 的 i32 值
pub fn soe_write_i32(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, value: i32, timeout_us: i32,
) -> Result<()> {
    soe_write_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, &value.to_le_bytes(), timeout_us)
}

/// 写入 SoE IDN 的 u16 值
pub fn soe_write_u16(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, value: u16, timeout_us: i32,
) -> Result<()> {
    soe_write_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, &value.to_le_bytes(), timeout_us)
}

/// 写入 SoE IDN 的 u32 值
pub fn soe_write_u32(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, value: u32, timeout_us: i32,
) -> Result<()> {
    soe_write_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, &value.to_le_bytes(), timeout_us)
}

/// 写入 SoE IDN 的 f32 值
pub fn soe_write_f32(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, value: f32, timeout_us: i32,
) -> Result<()> {
    soe_write_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, &value.to_bits().to_le_bytes(), timeout_us)
}

/// 写入 SoE IDN 的 f64 值
pub fn soe_write_f64(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, value: f64, timeout_us: i32,
) -> Result<()> {
    soe_write_raw(master_index, slave_index, idn, drive_no, SOE_ELEM_VALUE, &value.to_bits().to_le_bytes(), timeout_us)
}

// ===================== 内部原始读写 =====================

/// 读取 SoE IDN 原始字节
pub fn soe_read_raw(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, element_flags: u8, timeout_us: i32,
) -> Result<Vec<u8>> {
    let mut data_ptr: *mut std::ffi::c_void = std::ptr::null_mut();
    let mut data_size: c_int = 0;
    let ok = unsafe {
        ffi::SoERead(master_index, slave_index, drive_no, element_flags, idn,
                     &mut data_ptr, &mut data_size, timeout_us)
    };
    if ok == 0 || data_ptr.is_null() || data_size <= 0 {
        if !data_ptr.is_null() { unsafe { ffi::FreeMemory(data_ptr) }; }
        return Err(DarraError::SoeFailed(idn));
    }
    let data = unsafe {
        std::slice::from_raw_parts(data_ptr as *const u8, data_size as usize).to_vec()
    };
    unsafe { ffi::FreeMemory(data_ptr) };
    Ok(data)
}

/// 写入 SoE IDN 原始字节
pub fn soe_write_raw(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, element_flags: u8, data: &[u8], timeout_us: i32,
) -> Result<()> {
    let ok = unsafe {
        ffi::SoEWrite(master_index, slave_index, drive_no, element_flags, idn,
                      data.as_ptr(), data.len() as c_int, timeout_us)
    };
    if ok != 0 { Ok(()) } else { Err(DarraError::SoeFailed(idn)) }
}

/// 读取 SoE IDN 属性字
pub fn soe_read_attributes(
    master_index: u16, slave_index: u16,
    idn: u16, drive_no: u8, timeout_us: i32,
) -> Result<u32> {
    let mut attrs: u32 = 0;
    let ok = unsafe {
        ffi::SoEReadAttributes(master_index, slave_index, drive_no, idn, &mut attrs, timeout_us)
    };
    if ok != 0 { Ok(attrs) } else { Err(DarraError::SoeFailed(idn)) }
}

/// 获取从站支持的所有 IDN 列表
pub fn soe_list_idns(
    master_index: u16, slave_index: u16,
    drive_no: u8, timeout_us: i32,
) -> Result<Vec<u16>> {
    let mut list_ptr: *mut u16 = std::ptr::null_mut();
    let mut list_count: c_int = 0;
    let ok = unsafe {
        ffi::SoEReadIDNList(master_index, slave_index, drive_no,
                            &mut list_ptr, &mut list_count, timeout_us)
    };
    if ok == 0 || list_ptr.is_null() || list_count <= 0 {
        if !list_ptr.is_null() { unsafe { ffi::FreeMemory(list_ptr as *mut _) }; }
        return Err(DarraError::SoeFailed(0));
    }
    let idns = unsafe {
        std::slice::from_raw_parts(list_ptr, list_count as usize).to_vec()
    };
    unsafe { ffi::FreeMemory(list_ptr as *mut _) };
    Ok(idns)
}

/// 字节数组格式化为十六进制字符串
///
/// 对应 C# FormatByteArray
pub fn format_byte_array(data: &[u8]) -> String {
    if data.is_empty() {
        return String::new();
    }
    data.iter()
        .map(|b| format!("{:02X}", b))
        .collect::<Vec<_>>()
        .join(" ")
}

/// 根据数据类型智能格式化字节数组
///
/// data_type: 0=Binary, 1=UInt, 2=Int, 3=Hex, 4=String, 6=Float
pub fn format_byte_array_typed(data: &[u8], data_type: u8) -> String {
    if data.is_empty() {
        return String::new();
    }
    match data_type {
        1 => { // UInt
            match data.len() {
                1 => format!("{}", data[0]),
                2 => format!("{}", u16::from_le_bytes([data[0], data[1]])),
                4 => format!("{}", u32::from_le_bytes([data[0], data[1], data[2], data[3]])),
                _ => format_byte_array(data),
            }
        }
        2 => { // Int
            match data.len() {
                1 => format!("{}", data[0] as i8),
                2 => format!("{}", i16::from_le_bytes([data[0], data[1]])),
                4 => format!("{}", i32::from_le_bytes([data[0], data[1], data[2], data[3]])),
                _ => format_byte_array(data),
            }
        }
        3 => { // Hex
            format!("0x{}", data.iter().map(|b| format!("{:02X}", b)).collect::<String>())
        }
        4 => { // String
            // [2026-05-08 修复乱码] 格式化分支 String 走多编码兜底 (UTF-8/ASCII/Latin-1)
            let trimmed_end = data.iter().rposition(|&b| b != 0).map(|i| i + 1).unwrap_or(0);
            crate::utils::help::decode_ethercat_string(&data[..trimmed_end])
        }
        6 => { // Float
            match data.len() {
                4 => format!("{}", f32::from_le_bytes([data[0], data[1], data[2], data[3]])),
                8 => format!("{}", f64::from_le_bytes([data[0], data[1], data[2], data[3],
                                                        data[4], data[5], data[6], data[7]])),
                _ => format_byte_array(data),
            }
        }
        _ => format_byte_array(data),
    }
}

// ===================== 异步 API (轨道 1: std::thread, 无依赖) =====================

impl SoEInstance {
    /// SoE 读 IDN (std::thread 异步包装)
    pub fn read_blocking(
        master_index: u16,
        slave_index: u16,
        drive_number: u8,
        idn: u16,
        element_flags: u8,
        timeout_ms: i32,
    ) -> std::thread::JoinHandle<Result<Vec<u8>>> {
        std::thread::spawn(move || {
            let soe = SoEInstance::new(master_index, slave_index, drive_number);
            soe.read(idn, element_flags, timeout_ms)
        })
    }

    /// SoE 写 IDN (std::thread 异步包装)
    pub fn write_blocking(
        master_index: u16,
        slave_index: u16,
        drive_number: u8,
        idn: u16,
        data: Vec<u8>,
        element_flags: u8,
        timeout_ms: i32,
    ) -> std::thread::JoinHandle<Result<()>> {
        std::thread::spawn(move || {
            let soe = SoEInstance::new(master_index, slave_index, drive_number);
            soe.write(idn, &data, element_flags, timeout_ms)
        })
    }

    /// SoE 执行 SERCOS Procedure Command (std::thread 异步包装)
    pub fn execute_command_blocking(
        master_index: u16,
        slave_index: u16,
        drive_number: u8,
        idn: u16,
        timeout_ms: i32,
        poll_interval_ms: i32,
    ) -> std::thread::JoinHandle<Result<()>> {
        std::thread::spawn(move || {
            let soe = SoEInstance::new(master_index, slave_index, drive_number);
            soe.execute_command(idn, timeout_ms, poll_interval_ms)
        })
    }
}

// ===================== 异步 API (轨道 2: tokio, feature 门控) =====================

#[cfg(feature = "async-tokio")]
impl SoEInstance {
    /// SoE 读 IDN (tokio async)
    pub async fn read_async(&self, idn: u16, element_flags: u8, timeout_ms: i32) -> Result<Vec<u8>> {
        let master = self.master_index;
        let slave = self.slave_index;
        let drive = self.drive_number;
        tokio::task::spawn_blocking(move || {
            let soe = SoEInstance::new(master, slave, drive);
            soe.read(idn, element_flags, timeout_ms)
        })
        .await
        .map_err(|e| DarraError::Other(format!("tokio join error: {}", e)))?
    }

    /// SoE 写 IDN (tokio async)
    pub async fn write_async(&self, idn: u16, data: Vec<u8>, element_flags: u8, timeout_ms: i32) -> Result<()> {
        let master = self.master_index;
        let slave = self.slave_index;
        let drive = self.drive_number;
        tokio::task::spawn_blocking(move || {
            let soe = SoEInstance::new(master, slave, drive);
            soe.write(idn, &data, element_flags, timeout_ms)
        })
        .await
        .map_err(|e| DarraError::Other(format!("tokio join error: {}", e)))?
    }

    /// SoE 执行 SERCOS Procedure Command (tokio async)
    pub async fn execute_command_async(&self, idn: u16, timeout_ms: i32, poll_interval_ms: i32) -> Result<()> {
        let master = self.master_index;
        let slave = self.slave_index;
        let drive = self.drive_number;
        tokio::task::spawn_blocking(move || {
            let soe = SoEInstance::new(master, slave, drive);
            soe.execute_command(idn, timeout_ms, poll_interval_ms)
        })
        .await
        .map_err(|e| DarraError::Other(format!("tokio join error: {}", e)))?
    }
}

// ===================== IMailboxProtocol trait impl (对齐 C# SoEInstance) =====================

impl crate::abstractions::MailboxProtocol for SoEInstance {
    fn protocol_type(&self) -> u8 { 0x05 }
    fn protocol_name(&self) -> &'static str { "SoE" }

    fn is_supported(&self) -> bool {
        SoEInstance::is_supported(self)
    }

    fn last_error_code(&self) -> u32 {
        match self.last_soe_error {
            Some(e) => e as u32,
            None => 0,
        }
    }

    fn statistics(&self) -> crate::abstractions::MailboxStatistics {
        let mut stats = ffi::EcMbxStatsC::default();
        let rc = unsafe {
            ffi::mbx_get_stats_by_master(
                self.master_index, self.slave_index, 0x05, &mut stats,
            )
        };
        if rc == 1 {
            stats.into()
        } else {
            crate::abstractions::MailboxStatistics::empty()
        }
    }

    fn reset_statistics(&self) {
        unsafe {
            ffi::mbx_reset_stats_by_master(self.master_index, self.slave_index, 0x05);
        }
    }
}