darra_ethercat/slave/

coe.rs

use std::fmt;
use std::collections::HashMap;
use crate::data::error::{DarraError, Result};
use crate::utils::ffi;
use std::os::raw::c_int;

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[repr(u16)]
pub enum EcDataType {

    Unknown    = 0x0000,

    Boolean    = 0x0001,

    Integer8   = 0x0002,

    Integer16  = 0x0003,

    Integer32  = 0x0004,

    Unsigned8  = 0x0005,

    Unsigned16 = 0x0006,

    Unsigned32 = 0x0007,

    Real32     = 0x0008,

    VisibleString = 0x0009,

    OctetString = 0x000A,

    UnicodeString = 0x000B,

    TimeOfDay  = 0x000C,

    TimeDifference = 0x000D,

    Domain     = 0x000F,

    Integer24  = 0x0010,

    Real64     = 0x0011,

    Integer40  = 0x0012,

    Integer48  = 0x0013,

    Integer56  = 0x0014,

    Integer64  = 0x0015,

    Unsigned24 = 0x0016,

    Unsigned40 = 0x0018,

    Unsigned48 = 0x0019,

    Unsigned56 = 0x001A,

    Unsigned64 = 0x001B,

    Guid       = 0x001D,

    Byte       = 0x001E,

    Word       = 0x001F,

    DWord      = 0x0020,

    PdoMapping = 0x0021,

    SmComType  = 0x0022,

    Identity   = 0x0023,

    Bit1       = 0x0030,

    Bit2       = 0x0031,

    Bit3       = 0x0032,

    Bit4       = 0x0033,

    Bit5       = 0x0034,

    Bit6       = 0x0035,

    Bit7       = 0x0036,

    Bit8       = 0x0037,
}

impl EcDataType {

    pub fn from_raw(val: u16) -> Self {
        match val {
            0x0000 => Self::Unknown,
            0x0001 => Self::Boolean,
            0x0002 => Self::Integer8,
            0x0003 => Self::Integer16,
            0x0004 => Self::Integer32,
            0x0005 => Self::Unsigned8,
            0x0006 => Self::Unsigned16,
            0x0007 => Self::Unsigned32,
            0x0008 => Self::Real32,
            0x0009 => Self::VisibleString,
            0x000A => Self::OctetString,
            0x000B => Self::UnicodeString,
            0x000C => Self::TimeOfDay,
            0x000D => Self::TimeDifference,
            0x000F => Self::Domain,
            0x0010 => Self::Integer24,
            0x0011 => Self::Real64,
            0x0012 => Self::Integer40,
            0x0013 => Self::Integer48,
            0x0014 => Self::Integer56,
            0x0015 => Self::Integer64,
            0x0016 => Self::Unsigned24,
            0x0018 => Self::Unsigned40,
            0x0019 => Self::Unsigned48,
            0x001A => Self::Unsigned56,
            0x001B => Self::Unsigned64,
            0x001D => Self::Guid,
            0x001E => Self::Byte,
            0x001F => Self::Word,
            0x0020 => Self::DWord,
            0x0021 => Self::PdoMapping,
            0x0022 => Self::SmComType,
            0x0023 => Self::Identity,
            0x0030 => Self::Bit1,
            0x0031 => Self::Bit2,
            0x0032 => Self::Bit3,
            0x0033 => Self::Bit4,
            0x0034 => Self::Bit5,
            0x0035 => Self::Bit6,
            0x0036 => Self::Bit7,
            0x0037 => Self::Bit8,
            _ => Self::Unknown,
        }
    }

    pub fn byte_size(self) -> Option<usize> {
        match self {
            Self::Boolean | Self::Integer8 | Self::Unsigned8 | Self::Byte => Some(1),
            Self::Integer16 | Self::Unsigned16 | Self::Word => Some(2),
            Self::Integer24 | Self::Unsigned24 => Some(3),
            Self::Integer32 | Self::Unsigned32 | Self::Real32 | Self::DWord => Some(4),
            Self::Integer40 | Self::Unsigned40 => Some(5),
            Self::Integer48 | Self::Unsigned48 => Some(6),
            Self::Integer56 | Self::Unsigned56 => Some(7),
            Self::Integer64 | Self::Unsigned64 | Self::Real64 => Some(8),
            _ => None,
        }
    }
}

impl fmt::Display for EcDataType {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let name = match self {
            Self::Unknown       => "UNKNOWN",
            Self::Boolean       => "BOOL",
            Self::Integer8      => "INT8",
            Self::Integer16     => "INT16",
            Self::Integer32     => "INT32",
            Self::Integer64     => "INT64",
            Self::Integer24     => "INT24",
            Self::Integer40     => "INT40",
            Self::Integer48     => "INT48",
            Self::Integer56     => "INT56",
            Self::Unsigned8     => "UINT8",
            Self::Unsigned16    => "UINT16",
            Self::Unsigned32    => "UINT32",
            Self::Unsigned24    => "UINT24",
            Self::Unsigned40    => "UINT40",
            Self::Unsigned48    => "UINT48",
            Self::Unsigned56    => "UINT56",
            Self::Unsigned64    => "UINT64",
            Self::Real32        => "FLOAT",
            Self::Real64        => "DOUBLE",
            Self::VisibleString => "STRING",
            Self::OctetString   => "OCTET_STRING",
            Self::UnicodeString => "UNICODE_STRING",
            Self::TimeOfDay     => "TIME_OF_DAY",
            Self::TimeDifference => "TIME_DIFF",
            Self::Domain        => "DOMAIN",
            Self::Guid          => "GUID",
            Self::Byte          => "BYTE",
            Self::Word          => "WORD",
            Self::DWord         => "DWORD",
            Self::PdoMapping    => "PDO_MAP",
            Self::SmComType     => "SM_COM_TYPE",
            Self::Identity      => "IDENTITY",
            Self::Bit1          => "BIT1",
            Self::Bit2          => "BIT2",
            Self::Bit3          => "BIT3",
            Self::Bit4          => "BIT4",
            Self::Bit5          => "BIT5",
            Self::Bit6          => "BIT6",
            Self::Bit7          => "BIT7",
            Self::Bit8          => "BIT8",
        };
        write!(f, "{}", name)
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct ObjAccess(pub u16);

impl ObjAccess {

    pub const NONE:          u16 = 0x0000;

    pub const READ_PREOP:    u16 = 0x0001;

    pub const READ_SAFEOP:   u16 = 0x0002;

    pub const READ_OP:       u16 = 0x0004;

    pub const WRITE_PREOP:   u16 = 0x0008;

    pub const WRITE_SAFEOP:  u16 = 0x0010;

    pub const WRITE_OP:      u16 = 0x0020;

    pub const MAP_RXPDO:     u16 = 0x0040;

    pub const MAP_TXPDO:     u16 = 0x0080;

    pub const SETTINGS:      u16 = 0x0100;

    pub const READ_ANY:  u16 = Self::READ_PREOP | Self::READ_SAFEOP | Self::READ_OP;

    pub const WRITE_ANY: u16 = Self::WRITE_PREOP | Self::WRITE_SAFEOP | Self::WRITE_OP;

    pub fn can_read(self) -> bool {
        (self.0 & Self::READ_ANY) != 0
    }

    pub fn can_write(self) -> bool {
        (self.0 & Self::WRITE_ANY) != 0
    }

    pub fn is_read_only(self) -> bool {
        self.can_read() && !self.can_write()
    }

    pub fn can_write_preop(self) -> bool {
        (self.0 & Self::WRITE_PREOP) != 0
    }

    pub fn can_write_safeop(self) -> bool {
        (self.0 & (Self::WRITE_PREOP | Self::WRITE_SAFEOP)) != 0
    }

    pub fn can_write_op(self) -> bool {
        (self.0 & Self::WRITE_ANY) != 0
    }

    pub fn rxpdo_mappable(self) -> bool {
        (self.0 & Self::MAP_RXPDO) != 0
    }

    pub fn txpdo_mappable(self) -> bool {
        (self.0 & Self::MAP_TXPDO) != 0
    }

    pub fn readable(self) -> bool { self.can_read() }

    pub fn writable(self) -> bool { self.can_write() }

    pub fn access_description(self) -> String {
        let can_read = self.can_read();
        let can_write = self.can_write();
        if can_read && can_write {
            let mut parts = Vec::new();
            if (self.0 & Self::WRITE_PREOP) != 0 { parts.push("PreOP"); }
            if (self.0 & Self::WRITE_SAFEOP) != 0 { parts.push("SafeOP"); }
            if (self.0 & Self::WRITE_OP) != 0 { parts.push("OP"); }
            if parts.is_empty() {
                "读写".to_string()
            } else {
                format!("读写({}可写)", parts.join(","))
            }
        } else if can_read {
            "只读".to_string()
        } else if can_write {
            let mut parts = Vec::new();
            if (self.0 & Self::WRITE_PREOP) != 0 { parts.push("PreOP"); }
            if (self.0 & Self::WRITE_SAFEOP) != 0 { parts.push("SafeOP"); }
            if (self.0 & Self::WRITE_OP) != 0 { parts.push("OP"); }
            if parts.is_empty() {
                "只写".to_string()
            } else {
                format!("只写({}可写)", parts.join(","))
            }
        } else {
            "无权限".to_string()
        }
    }
}

impl fmt::Display for ObjAccess {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let mut parts = Vec::new();
        if (self.0 & Self::READ_PREOP) != 0  { parts.push("R(PreOp)"); }
        if (self.0 & Self::READ_SAFEOP) != 0 { parts.push("R(SafeOp)"); }
        if (self.0 & Self::READ_OP) != 0     { parts.push("R(Op)"); }
        if (self.0 & Self::WRITE_PREOP) != 0  { parts.push("W(PreOp)"); }
        if (self.0 & Self::WRITE_SAFEOP) != 0 { parts.push("W(SafeOp)"); }
        if (self.0 & Self::WRITE_OP) != 0     { parts.push("W(Op)"); }
        if (self.0 & Self::MAP_RXPDO) != 0   { parts.push("RxPDO"); }
        if (self.0 & Self::MAP_TXPDO) != 0   { parts.push("TxPDO"); }
        write!(f, "[{}]", parts.join("|"))
    }
}

#[derive(Debug, Clone)]
pub struct DiagnosticMessage {

    pub sub_index: u8,

    pub diag_code: u32,

    pub flags: u16,

    pub text_index: u16,

    pub raw_data: Vec<u8>,
}

impl fmt::Display for DiagnosticMessage {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "Diag[{}]: Code=0x{:08X}, Flags=0x{:04X}",
               self.sub_index, self.diag_code, self.flags)
    }
}

#[derive(Debug, Clone)]
pub struct CoEAccessDenied {
    pub index: u16,
    pub sub_index: u8,
    pub entry_name: String,
    pub is_read_operation: bool,
    pub obj_access: u16,
    pub message: String,
}

impl CoEAccessDenied {

    pub fn new(index: u16, sub_index: u8, entry_name: &str,
               is_read_operation: bool, obj_access: u16) -> Self {
        let operation = if is_read_operation { "读取" } else { "写入" };
        let access_desc = ObjAccess(obj_access).access_description();
        let message = format!(
            "对象条目 0x{:04X}:{:02X} ({}) 不支持{}操作。访问权限: {}",
            index, sub_index, entry_name, operation, access_desc
        );
        Self { index, sub_index, entry_name: entry_name.to_string(),
               is_read_operation, obj_access, message }
    }

    pub fn create_with_custom_message(
        index: u16, sub_index: u8, custom_message: &str,
        is_read_operation: bool, obj_access: u16,
    ) -> Self {
        Self { index, sub_index, entry_name: custom_message.to_string(),
               is_read_operation, obj_access, message: custom_message.to_string() }
    }
}

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

impl std::error::Error for CoEAccessDenied {}

#[derive(Debug, Clone)]
pub struct ObjectEntry {

    pub od_index: u16,

    pub sub_index: u8,

    pub name: String,

    pub data_type: EcDataType,

    pub bit_length: u16,

    pub access: ObjAccess,

    pub value_info: u8,
}

impl ObjectEntry {

    pub fn byte_length(&self) -> usize {
        ((self.bit_length as usize) + 7) / 8
    }

    pub fn datatype(&self) -> EcDataType {
        self.data_type
    }

    pub fn can_read(&self) -> bool { self.access.can_read() }

    pub fn can_write(&self) -> bool { self.access.can_write() }

    pub fn is_read_only(&self) -> bool { self.access.is_read_only() }

    pub fn can_write_preop(&self) -> bool { self.access.can_write_preop() }

    pub fn can_write_safeop(&self) -> bool { self.access.can_write_safeop() }

    pub fn can_write_op(&self) -> bool { self.access.can_write_op() }

    pub fn access_description(&self) -> String {
        self.access.access_description()
    }

    pub fn read_raw(&self, master_index: u16, slave_index: u16) -> Result<Vec<u8>> {
        if !self.can_read() {
            return Err(DarraError::SdoReadFailed {
                index: self.od_index, subindex: self.sub_index, abort_code: None });
        }
        let mut size: c_int = 0;
        let ptr = unsafe {
            ffi::SDOread(master_index, slave_index, self.od_index, self.sub_index, 0, &mut size)
        };
        if ptr.is_null() || size <= 0 {
            if !ptr.is_null() {
                unsafe { ffi::FreeMemory(ptr as *mut _) };
            }
            return Err(DarraError::SdoReadFailed {
                index: self.od_index, subindex: self.sub_index, abort_code: None });
        }
        let data = unsafe { std::slice::from_raw_parts(ptr, size as usize).to_vec() };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(data)
    }

    pub fn bytes(&self, master_index: u16, slave_index: u16) -> Result<Vec<u8>> {
        self.read_raw(master_index, slave_index)
    }

    pub fn read_u8(&self, master_index: u16, slave_index: u16) -> Result<u8> {
        let data = self.read_raw(master_index, slave_index)?;
        data.first().copied().ok_or(DarraError::SdoReadFailed {
            index: self.od_index, subindex: self.sub_index, abort_code: None })
    }

    pub fn read_u16(&self, master_index: u16, slave_index: u16) -> Result<u16> {
        let data = self.read_raw(master_index, slave_index)?;
        if data.len() < 2 {
            return Err(DarraError::SdoReadFailed { index: self.od_index, subindex: self.sub_index, abort_code: None });
        }
        Ok(u16::from_le_bytes([data[0], data[1]]))
    }

    pub fn read_u32(&self, master_index: u16, slave_index: u16) -> Result<u32> {
        let data = self.read_raw(master_index, slave_index)?;
        if data.len() < 4 {
            return Err(DarraError::SdoReadFailed { index: self.od_index, subindex: self.sub_index, abort_code: None });
        }
        Ok(u32::from_le_bytes([data[0], data[1], data[2], data[3]]))
    }

    pub fn read_u64(&self, master_index: u16, slave_index: u16) -> Result<u64> {
        let data = self.read_raw(master_index, slave_index)?;
        if data.len() < 8 {
            return Err(DarraError::SdoReadFailed { index: self.od_index, subindex: self.sub_index, abort_code: None });
        }
        Ok(u64::from_le_bytes([
            data[0], data[1], data[2], data[3],
            data[4], data[5], data[6], data[7],
        ]))
    }

    pub fn read_i32(&self, master_index: u16, slave_index: u16) -> Result<i32> {
        Ok(self.read_u32(master_index, slave_index)? as i32)
    }

    pub fn read_i64(&self, master_index: u16, slave_index: u16) -> Result<i64> {
        Ok(self.read_u64(master_index, slave_index)? as i64)
    }

    pub fn read_f32(&self, master_index: u16, slave_index: u16) -> Result<f32> {
        let bits = self.read_u32(master_index, slave_index)?;
        Ok(f32::from_bits(bits))
    }

    pub fn read_f64(&self, master_index: u16, slave_index: u16) -> Result<f64> {
        let bits = self.read_u64(master_index, slave_index)?;
        Ok(f64::from_bits(bits))
    }

    pub fn read_string(&self, master_index: u16, slave_index: u16) -> Result<String> {
        let data = self.read_raw(master_index, slave_index)?;
        let end = data.iter().position(|&b| b == 0).unwrap_or(data.len());
        Ok(crate::utils::help::decode_ethercat_string(&data[..end]))
    }

    pub fn get_value(&self, master_index: u16, slave_index: u16) -> Result<CoEValue> {
        match self.data_type {
            EcDataType::Boolean => {
                let v = self.read_u8(master_index, slave_index)?;
                Ok(CoEValue::Bool(v != 0))
            }
            EcDataType::Integer8 => {
                let v = self.read_u8(master_index, slave_index)?;
                Ok(CoEValue::I8(v as i8))
            }
            EcDataType::Unsigned8 | EcDataType::Byte => {
                Ok(CoEValue::U8(self.read_u8(master_index, slave_index)?))
            }
            EcDataType::Integer16 => {
                let v = self.read_u16(master_index, slave_index)?;
                Ok(CoEValue::I16(v as i16))
            }
            EcDataType::Unsigned16 | EcDataType::Word => {
                Ok(CoEValue::U16(self.read_u16(master_index, slave_index)?))
            }
            EcDataType::Integer32 | EcDataType::Integer24 => {
                Ok(CoEValue::I32(self.read_i32(master_index, slave_index)?))
            }
            EcDataType::Unsigned32 | EcDataType::Unsigned24 | EcDataType::DWord => {
                Ok(CoEValue::U32(self.read_u32(master_index, slave_index)?))
            }
            EcDataType::Integer64 => {
                Ok(CoEValue::I64(self.read_i64(master_index, slave_index)?))
            }
            EcDataType::Unsigned64 => {
                Ok(CoEValue::U64(self.read_u64(master_index, slave_index)?))
            }
            EcDataType::Real32 => {
                Ok(CoEValue::F32(self.read_f32(master_index, slave_index)?))
            }
            EcDataType::Real64 => {
                Ok(CoEValue::F64(self.read_f64(master_index, slave_index)?))
            }
            EcDataType::VisibleString | EcDataType::UnicodeString => {
                Ok(CoEValue::Str(self.read_string(master_index, slave_index)?))
            }
            _ => {
                Ok(CoEValue::Raw(self.read_raw(master_index, slave_index)?))
            }
        }
    }

    pub fn write_raw(&self, master_index: u16, slave_index: u16, data: &[u8]) -> Result<()> {
        if !self.can_write() {
            return Err(DarraError::SdoWriteFailed {
                index: self.od_index, subindex: self.sub_index, abort_code: None });
        }
        let ok = unsafe {
            ffi::SDOwrite_raw(master_index, slave_index, self.od_index, self.sub_index,
                              0, data.as_ptr(), data.len() as c_int)
        };
        if ok != 0 {
            Ok(())
        } else {
            Err(DarraError::SdoWriteFailed { index: self.od_index, subindex: self.sub_index, abort_code: None })
        }
    }

    pub fn write_u8(&self, master_index: u16, slave_index: u16, value: u8) -> Result<()> {
        self.write_raw(master_index, slave_index, &[value])
    }

    pub fn write_u16(&self, master_index: u16, slave_index: u16, value: u16) -> Result<()> {
        self.write_raw(master_index, slave_index, &value.to_le_bytes())
    }

    pub fn write_u32(&self, master_index: u16, slave_index: u16, value: u32) -> Result<()> {
        self.write_raw(master_index, slave_index, &value.to_le_bytes())
    }

    pub fn write_u64(&self, master_index: u16, slave_index: u16, value: u64) -> Result<()> {
        self.write_raw(master_index, slave_index, &value.to_le_bytes())
    }

    pub fn write_i32(&self, master_index: u16, slave_index: u16, value: i32) -> Result<()> {
        self.write_raw(master_index, slave_index, &value.to_le_bytes())
    }

    pub fn write_i64(&self, master_index: u16, slave_index: u16, value: i64) -> Result<()> {
        self.write_raw(master_index, slave_index, &value.to_le_bytes())
    }

    pub fn write_f32(&self, master_index: u16, slave_index: u16, value: f32) -> Result<()> {
        self.write_raw(master_index, slave_index, &value.to_bits().to_le_bytes())
    }

    pub fn write_f64(&self, master_index: u16, slave_index: u16, value: f64) -> Result<()> {
        self.write_raw(master_index, slave_index, &value.to_bits().to_le_bytes())
    }
}

impl fmt::Display for ObjectEntry {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f,
            "  [{:02X}] {:40} {:12} {}bit {}",
            self.sub_index, self.name, self.data_type, self.bit_length, self.access
        )
    }
}

#[derive(Debug, Clone)]
pub enum CoEValue {
    Bool(bool),
    I8(i8),
    U8(u8),
    I16(i16),
    U16(u16),
    I32(i32),
    U32(u32),
    I64(i64),
    U64(u64),
    F32(f32),
    F64(f64),
    Str(String),
    Raw(Vec<u8>),
}

impl fmt::Display for CoEValue {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            CoEValue::Bool(v) => write!(f, "{}", v),
            CoEValue::I8(v) => write!(f, "{}", v),
            CoEValue::U8(v) => write!(f, "{}", v),
            CoEValue::I16(v) => write!(f, "{}", v),
            CoEValue::U16(v) => write!(f, "{}", v),
            CoEValue::I32(v) => write!(f, "{}", v),
            CoEValue::U32(v) => write!(f, "{}", v),
            CoEValue::I64(v) => write!(f, "{}", v),
            CoEValue::U64(v) => write!(f, "{}", v),
            CoEValue::F32(v) => write!(f, "{:.6}", v),
            CoEValue::F64(v) => write!(f, "{:.6}", v),
            CoEValue::Str(v) => write!(f, "{}", v),
            CoEValue::Raw(v) => {
                let hex: Vec<String> = v.iter().map(|b| format!("{:02X}", b)).collect();
                write!(f, "{}", hex.join(" "))
            }
        }
    }
}

#[derive(Debug, Clone)]
pub struct OdObject {

    pub index: u16,

    pub name: String,

    pub object_code: u8,

    pub data_type: EcDataType,

    pub max_sub: u8,

    pub entries: Vec<ObjectEntry>,
}

impl OdObject {

    pub fn count(&self) -> usize {
        self.entries.len()
    }

    pub fn datatype(&self) -> EcDataType {
        self.data_type
    }

    pub fn is_var(&self) -> bool {
        self.object_code == 0x07
    }

    pub fn is_array(&self) -> bool {
        self.object_code == 0x08
    }

    pub fn is_record(&self) -> bool {
        self.object_code == 0x09
    }

    pub fn entry(&self, sub_index: u8) -> Option<&ObjectEntry> {
        self.entries.iter().find(|e| e.sub_index == sub_index)
    }

    pub fn contains_key(&self, sub_index: u8) -> bool {
        self.entries.iter().any(|e| e.sub_index == sub_index)
    }

    pub fn entry_by_name(&self, name: &str) -> Option<&ObjectEntry> {
        let name_lower = name.to_lowercase();
        self.entries.iter().find(|e| e.name.to_lowercase() == name_lower)
    }

    pub fn get_by_position(&self, position: usize) -> Option<&ObjectEntry> {
        self.entries.get(position)
    }

    pub fn read_all(&self, master_index: u16, slave_index: u16) -> HashMap<u8, Vec<u8>> {
        let mut result = HashMap::new();
        for entry in &self.entries {
            if let Ok(data) = entry.read_raw(master_index, slave_index) {
                if !data.is_empty() {
                    result.insert(entry.sub_index, data);
                }
            }
        }
        result
    }

    pub fn copy(&self) -> Self {
        self.clone()
    }
}

impl fmt::Display for OdObject {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let kind = match self.object_code {
            0x07 => "VAR   ",
            0x08 => "ARRAY ",
            0x09 => "RECORD",
            _    => "OTHER ",
        };
        writeln!(f, "[0x{:04X}] {} {} ({}条目)", self.index, kind, self.name, self.entries.len())?;
        for entry in &self.entries {
            writeln!(f, "{}", entry)?;
        }
        Ok(())
    }
}

pub struct OdList {

    pub master_index: u16,

    pub slave_index: u16,

    pub objects: Vec<OdObject>,
}

impl OdList {

    pub fn load(master_index: u16, slave_index: u16) -> Result<Self> {
        let odlist_ptr = unsafe { ffi::GetSlaveSDOList(master_index, slave_index) };
        if odlist_ptr.is_null() {
            return Err(DarraError::SdoReadFailed { index: 0, subindex: 0, abort_code: None });
        }

        let objects = Self::parse_odlist_objects(master_index, slave_index, odlist_ptr, true)?;

        unsafe { ffi::FreeMemory(odlist_ptr as *mut std::os::raw::c_void) };
        Ok(Self { master_index, slave_index, objects })
    }

    pub fn load_basic(master_index: u16, slave_index: u16) -> Result<Self> {
        let odlist_ptr = unsafe { ffi::GetSlaveSDOListBasic(master_index, slave_index) };
        if odlist_ptr.is_null() {
            return Err(DarraError::SdoReadFailed { index: 0, subindex: 0, abort_code: None });
        }

        let objects = Self::parse_odlist_objects(master_index, slave_index, odlist_ptr, false)?;
        unsafe { ffi::FreeMemory(odlist_ptr as *mut _) };
        Ok(Self { master_index, slave_index, objects })
    }

    pub(crate) fn from_raw_ptr(
        master_index: u16, slave_index: u16, odlist_ptr: *const std::ffi::c_void,
    ) -> Result<Self> {
        if odlist_ptr.is_null() {
            return Err(DarraError::SdoReadFailed { index: 0, subindex: 0, abort_code: None });
        }
        let objects = Self::parse_odlist_objects(master_index, slave_index, odlist_ptr, true)?;
        Ok(Self { master_index, slave_index, objects })
    }

    fn parse_odlist_objects(
        master_index: u16, slave_index: u16,
        odlist_ptr: *const std::ffi::c_void, load_entries: bool,
    ) -> Result<Vec<OdObject>> {
        let base = odlist_ptr as *const u8;

        let entries_count = unsafe {
            let p = base.add(2) as *const u16;
            std::ptr::read_unaligned(p) as usize
        };
        let entries_count = entries_count.min(1024);

        let mut objects = Vec::with_capacity(entries_count);

        for i in 0..entries_count {
            let idx_val = unsafe {
                let p = base.add(4 + i * 2) as *const u16;
                std::ptr::read_unaligned(p)
            };
            let dt_raw = unsafe {
                let p = base.add(2052 + i * 2) as *const u16;
                std::ptr::read_unaligned(p)
            };
            let obj_code = unsafe { *base.add(4100 + i) };
            let max_sub = unsafe { *base.add(5124 + i) };
            let name = {
                let name_ptr = unsafe { base.add(6148 + i * 41) };
                let name_slice = unsafe { std::slice::from_raw_parts(name_ptr, 41) };
                let end = name_slice.iter().position(|&b| b == 0).unwrap_or(41);

                crate::utils::help::decode_ethercat_string(&name_slice[..end])
            };

            let mut entries = Vec::new();
            if load_entries {
                let oe_ptr = unsafe {
                    ffi::GetSlavePointer_SDO_WithODList(master_index, slave_index, i as u16, odlist_ptr)
                };
                if !oe_ptr.is_null() {
                    entries = parse_oe_entries(oe_ptr, idx_val, max_sub);
                    unsafe { ffi::FreeMemory(oe_ptr as *mut std::os::raw::c_void) };
                }
            }

            objects.push(OdObject {
                index: idx_val,
                name,
                object_code: obj_code,
                data_type: EcDataType::from_raw(dt_raw),
                max_sub,
                entries,
            });
        }

        Ok(objects)
    }

    pub fn count(&self) -> usize {
        self.objects.len()
    }

    pub fn find(&self, index: u16) -> Option<&OdObject> {
        self.objects.iter().find(|o| o.index == index)
    }

    pub fn contains_key(&self, index: u16) -> bool {
        self.objects.iter().any(|o| o.index == index)
    }

    pub fn find_by_name(&self, name: &str) -> Vec<&OdObject> {
        let name_lower = name.to_lowercase();
        self.objects.iter()
            .filter(|o| o.name.to_lowercase().contains(&name_lower))
            .collect()
    }

    pub fn read_entry_raw(
        &self, index: u16, sub_index: u8,
    ) -> Result<Vec<u8>> {
        let mut size: c_int = 0;
        let ptr = unsafe {
            ffi::SDOread(self.master_index, self.slave_index, index, sub_index, 0, &mut size)
        };
        if ptr.is_null() || size <= 0 {
            if !ptr.is_null() {
                unsafe { ffi::FreeMemory(ptr as *mut _) };
            }
            return Err(DarraError::SdoReadFailed { index, subindex: sub_index, abort_code: None });
        }
        let data = unsafe { std::slice::from_raw_parts(ptr, size as usize).to_vec() };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(data)
    }

    pub fn get_by_position(&self, position: usize) -> Option<&OdObject> {
        self.objects.get(position)
    }

    pub fn len(&self) -> usize {
        self.objects.len()
    }

    pub fn is_empty(&self) -> bool {
        self.objects.is_empty()
    }

    pub fn copy(&self) -> Self {
        Self {
            master_index: self.master_index,
            slave_index: self.slave_index,
            objects: self.objects.clone(),
        }
    }

    pub fn keys(&self) -> Vec<u16> {
        self.objects.iter().map(|o| o.index).collect()
    }

    pub fn load_via_coe_h2(master_index: u16, slave_index: u16) -> Result<Self> {
        let odlist_ptr = unsafe { ffi::coe_get_od_list(master_index, slave_index) };
        if odlist_ptr.is_null() {
            return Err(DarraError::SdoReadFailed { index: 0, subindex: 0, abort_code: None });
        }

        let mut objects: Vec<OdObject> = Vec::new();
        unsafe {
            let base = odlist_ptr as *const u8;
            let entries_count = {
                let p = base.add(2) as *const u16;
                std::ptr::read_unaligned(p) as usize
            }.min(1024);

            for i in 0..entries_count {
                let idx_val = {
                    let p = base.add(4 + i * 2) as *const u16;
                    std::ptr::read_unaligned(p)
                };
                if idx_val == 0 { continue; }
                let dt_raw = {
                    let p = base.add(2052 + i * 2) as *const u16;
                    std::ptr::read_unaligned(p)
                };
                let obj_code = *base.add(4100 + i);
                let max_sub = *base.add(5124 + i);
                let name = {
                    let name_ptr = base.add(6148 + i * 41);
                    let name_slice = std::slice::from_raw_parts(name_ptr, 41);
                    let end = name_slice.iter().position(|&b| b == 0).unwrap_or(41);
                    crate::utils::help::decode_ethercat_string(&name_slice[..end])
                };

                let mut entries = Vec::new();
                for s in 0..=max_sub {
                    let oe_ptr = ffi::coe_get_entry_desc(master_index, slave_index, idx_val, s);
                    if oe_ptr.is_null() { continue; }
                    let ob = oe_ptr as *const u8;
                    let edt = {
                        let p = ob.add(258) as *const u16;
                        std::ptr::read_unaligned(p)
                    };
                    let bit_length = {
                        let p = ob.add(770) as *const u16;
                        std::ptr::read_unaligned(p)
                    };
                    let access_raw = {
                        let p = ob.add(1282) as *const u16;
                        std::ptr::read_unaligned(p)
                    };
                    let ename = {
                        let name_ptr = ob.add(1794);
                        let name_slice = std::slice::from_raw_parts(name_ptr, 41);
                        let end = name_slice.iter().position(|&b| b == 0).unwrap_or(41);
                        crate::utils::help::decode_ethercat_string(&name_slice[..end])
                    };
                    entries.push(ObjectEntry {
                        od_index: idx_val,
                        sub_index: s,
                        name: ename,
                        data_type: EcDataType::from_raw(edt),
                        bit_length,
                        access: ObjAccess(access_raw),
                        value_info: 0,
                    });
                    ffi::coe_free_oelist(oe_ptr);
                }

                objects.push(OdObject {
                    index: idx_val,
                    name,
                    object_code: obj_code,
                    data_type: EcDataType::from_raw(dt_raw),
                    max_sub,
                    entries,
                });
            }

            ffi::coe_free_odlist(odlist_ptr);
        }
        Ok(Self { master_index, slave_index, objects })
    }

    pub fn get_object_description(master_index: u16, slave_index: u16, index: u16)
        -> Option<OdObject>
    {
        let p = unsafe { ffi::coe_get_object_desc(master_index, slave_index, index) };
        if p.is_null() { return None; }
        let result = unsafe {
            let base = p as *const u8;
            let dt_raw = {
                let pp = base.add(2052) as *const u16;
                std::ptr::read_unaligned(pp)
            };
            let obj_code = *base.add(4100);
            let max_sub = *base.add(5124);
            let name = {
                let name_ptr = base.add(6148);
                let name_slice = std::slice::from_raw_parts(name_ptr, 41);
                let end = name_slice.iter().position(|&b| b == 0).unwrap_or(41);
                crate::utils::help::decode_ethercat_string(&name_slice[..end])
            };
            OdObject {
                index,
                name,
                object_code: obj_code,
                data_type: EcDataType::from_raw(dt_raw),
                max_sub,
                entries: Vec::new(),
            }
        };
        unsafe { ffi::coe_free_odlist(p); }
        Some(result)
    }

    pub fn get_entry_description(
        master_index: u16, slave_index: u16, index: u16, subindex: u8,
    ) -> Option<ObjectEntry> {
        let p = unsafe { ffi::coe_get_entry_desc(master_index, slave_index, index, subindex) };
        if p.is_null() { return None; }
        let result = unsafe {
            let base = p as *const u8;
            let edt = {
                let pp = base.add(258) as *const u16;
                std::ptr::read_unaligned(pp)
            };
            let bit_length = {
                let pp = base.add(770) as *const u16;
                std::ptr::read_unaligned(pp)
            };
            let access_raw = {
                let pp = base.add(1282) as *const u16;
                std::ptr::read_unaligned(pp)
            };
            let name = {
                let name_ptr = base.add(1794);
                let name_slice = std::slice::from_raw_parts(name_ptr, 41);
                let end = name_slice.iter().position(|&b| b == 0).unwrap_or(41);
                crate::utils::help::decode_ethercat_string(&name_slice[..end])
            };
            ObjectEntry {
                od_index: index,
                sub_index: subindex,
                name,
                data_type: EcDataType::from_raw(edt),
                bit_length,
                access: ObjAccess(access_raw),
                value_info: 0,
            }
        };
        unsafe { ffi::coe_free_oelist(p); }
        Some(result)
    }
}

impl fmt::Display for OdList {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        writeln!(f, "从站 {} 对象字典 ({} 个对象):", self.slave_index, self.objects.len())?;
        writeln!(f, "{}", "=".repeat(72))?;
        for obj in &self.objects {
            write!(f, "{}", obj)?;
        }
        Ok(())
    }
}

pub fn read_diagnostic_messages(master_index: u16, slave_index: u16) -> Vec<DiagnosticMessage> {
    let mut messages = Vec::new();

    let mut size: c_int = 0;
    let ptr = unsafe {
        ffi::SDOread(master_index, slave_index, 0x10F3, 0, 0, &mut size)
    };
    if ptr.is_null() || size < 1 {
        if !ptr.is_null() {
            unsafe { ffi::FreeMemory(ptr as *mut _) };
        }
        return messages;
    }
    let count = unsafe { *ptr } as usize;
    unsafe { ffi::FreeMemory(ptr as *mut _) };

    for i in 1..=count.min(20) {
        let mut msg_size: c_int = 0;
        let msg_ptr = unsafe {
            ffi::SDOread(master_index, slave_index, 0x10F3, i as u8, 0, &mut msg_size)
        };
        if msg_ptr.is_null() || msg_size < 8 {
            if !msg_ptr.is_null() {
                unsafe { ffi::FreeMemory(msg_ptr as *mut _) };
            }
            continue;
        }

        let data = unsafe { std::slice::from_raw_parts(msg_ptr, msg_size as usize).to_vec() };
        unsafe { ffi::FreeMemory(msg_ptr as *mut _) };

        let diag_code = u32::from_le_bytes([data[0], data[1], data[2], data[3]]);
        let flags = u16::from_le_bytes([data[4], data[5]]);
        let text_index = u16::from_le_bytes([data[6], data[7]]);

        messages.push(DiagnosticMessage {
            sub_index: i as u8,
            diag_code,
            flags,
            text_index,
            raw_data: data,
        });
    }

    messages
}

pub fn get_device_profile(master_index: u16, slave_index: u16) -> u16 {
    let mut size: c_int = 0;
    let ptr = unsafe {
        ffi::SDOread(master_index, slave_index, 0x1000, 0, 0, &mut size)
    };
    if ptr.is_null() || size < 4 {
        if !ptr.is_null() {
            unsafe { ffi::FreeMemory(ptr as *mut _) };
        }
        return 0;
    }
    let data = unsafe { std::slice::from_raw_parts(ptr, 4) };
    let device_type = u32::from_le_bytes([data[0], data[1], data[2], data[3]]);
    unsafe { ffi::FreeMemory(ptr as *mut _) };
    (device_type & 0xFFFF) as u16
}

fn parse_oe_entries(oe_ptr: *const std::ffi::c_void, od_index: u16, max_sub: u8) -> Vec<ObjectEntry> {
    let base = oe_ptr as *const u8;

    let oe_entries = unsafe {
        let p = base as *const u16;
        std::ptr::read_unaligned(p) as usize
    };
    let count = oe_entries.min(max_sub as usize + 1).min(256);

    let mut entries = Vec::with_capacity(count);
    for i in 0..count {
        let value_info = unsafe { *base.add(2 + i) };
        let dt_raw = unsafe {
            let p = base.add(258 + i * 2) as *const u16;
            std::ptr::read_unaligned(p)
        };
        let bit_length = unsafe {
            let p = base.add(770 + i * 2) as *const u16;
            std::ptr::read_unaligned(p)
        };
        let access_raw = unsafe {
            let p = base.add(1282 + i * 2) as *const u16;
            std::ptr::read_unaligned(p)
        };
        let name = {
            let name_ptr = unsafe { base.add(1794 + i * 41) };
            let name_slice = unsafe { std::slice::from_raw_parts(name_ptr, 41) };
            let end = name_slice.iter().position(|&b| b == 0).unwrap_or(41);

            crate::utils::help::decode_ethercat_string(&name_slice[..end])
        };

        entries.push(ObjectEntry {
            od_index,
            sub_index: i as u8,
            name,
            data_type: EcDataType::from_raw(dt_raw),
            bit_length,
            access: ObjAccess(access_raw),
            value_info,
        });
    }
    entries
}

use crate::data::types::SDOError;
use std::cell::Cell;

#[derive(Debug, Clone)]
pub struct SdoReadResultEx {

    pub data: Option<Vec<u8>>,

    pub abort_code: u32,

    pub wkc: i32,
}

impl SdoReadResultEx {

    pub fn is_ok(&self) -> bool { self.data.is_some() }
}

#[derive(Debug, Clone, Copy)]
pub struct SdoWriteResultEx {

    pub ok: bool,

    pub abort_code: u32,

    pub wkc: i32,
}

pub struct CoEInstance {
    master_index: u16,
    slave_index: u16,

    last_sdo_error: Cell<SDOError>,
}

impl CoEInstance {

    pub fn new(master_index: u16, slave_index: u16) -> Self {
        Self {
            master_index,
            slave_index,
            last_sdo_error: Cell::new(SDOError::NoError),
        }
    }

    pub fn last_sdo_error(&self) -> SDOError {
        self.last_sdo_error.get()
    }

    pub fn is_supported(&self) -> bool {
        let proto = unsafe { ffi::GetSlaveMailboxProto(self.master_index, self.slave_index) };
        (proto & 0x04) != 0
    }

    pub fn sdo_read(&self, index: u16, subindex: u8, complete_access: bool) -> Result<Vec<u8>> {
        let mut size: c_int = 0;
        let ptr = unsafe {
            ffi::SDOread(self.master_index, self.slave_index, index, subindex,
                         if complete_access { 1 } else { 0 }, &mut size)
        };
        if ptr.is_null() || size <= 0 {

            let abort = if size < 0 {
                let abort_code = (-size) as u32;
                let e = SDOError::from_u32(abort_code);
                self.last_sdo_error.set(e);
                Some(e)
            } else {
                self.last_sdo_error.set(SDOError::GeneralError);
                Some(SDOError::GeneralError)
            };
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            return Err(DarraError::SdoReadFailed { index, subindex, abort_code: abort });
        }
        let data = unsafe { std::slice::from_raw_parts(ptr, size as usize).to_vec() };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        self.last_sdo_error.set(SDOError::NoError);
        Ok(data)
    }

    pub fn sdo_write(&self, index: u16, subindex: u8, complete_access: bool, data: &[u8]) -> Result<()> {
        let ok = unsafe {
            ffi::SDOwrite_raw(self.master_index, self.slave_index, index, subindex,
                              if complete_access { 1 } else { 0 }, data.as_ptr(), data.len() as c_int)
        };
        if ok == 0 {
            self.last_sdo_error.set(SDOError::GeneralError);
            let mut abort_opt: Option<SDOError> = Some(SDOError::GeneralError);

            let mut probe_size: c_int = 0;
            let probe = unsafe {
                ffi::SDOread(self.master_index, self.slave_index, index, subindex, 0, &mut probe_size)
            };
            if probe.is_null() && probe_size < 0 {
                let abort_code = (-probe_size) as u32;
                let e = SDOError::from_u32(abort_code);
                self.last_sdo_error.set(e);
                abort_opt = Some(e);
            } else if !probe.is_null() {
                unsafe { ffi::FreeMemory(probe as *mut _) };
            }
            Err(DarraError::SdoWriteFailed { index, subindex, abort_code: abort_opt })
        } else {
            self.last_sdo_error.set(SDOError::NoError);
            Ok(())
        }
    }

    pub fn sdo_read_ex(&self, index: u16, subindex: u8, complete_access: bool) -> SdoReadResultEx {
        let mut size: c_int = 0;
        let mut abort_code: u32 = 0;
        let mut wkc: c_int = 0;
        let ptr = unsafe {
            ffi::SDOread_ex(
                self.master_index, self.slave_index, index, subindex,
                if complete_access { 1 } else { 0 },
                &mut size, &mut abort_code, &mut wkc,
            )
        };
        if ptr.is_null() || size <= 0 {
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            self.last_sdo_error.set(SDOError::from_u32(abort_code));
            return SdoReadResultEx { data: None, abort_code, wkc };
        }
        let data = unsafe { std::slice::from_raw_parts(ptr, size as usize).to_vec() };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        self.last_sdo_error.set(SDOError::NoError);
        SdoReadResultEx { data: Some(data), abort_code, wkc }
    }

    pub fn sdo_write_ex(&self, index: u16, subindex: u8, complete_access: bool, data: &[u8]) -> SdoWriteResultEx {
        let mut abort_code: u32 = 0;
        let mut wkc: c_int = 0;
        let ret = unsafe {
            ffi::SDOwrite_ex(
                self.master_index, self.slave_index, index, subindex,
                if complete_access { 1 } else { 0 },
                data.as_ptr(), data.len() as c_int,
                &mut abort_code, &mut wkc,
            )
        };
        let ok = ret != 0;
        if ok {
            self.last_sdo_error.set(SDOError::NoError);
        } else {
            self.last_sdo_error.set(SDOError::from_u32(abort_code));
        }
        SdoWriteResultEx { ok, abort_code, wkc }
    }

    pub fn read_multiple(&self, entries: &[(u16, u8)]) -> HashMap<(u16, u8), Option<Vec<u8>>> {
        let mut results = HashMap::new();
        for &(index, subindex) in entries {
            let data = self.sdo_read(index, subindex, false).ok();
            results.insert((index, subindex), data);
        }
        results
    }

    pub fn master_index(&self) -> u16 { self.master_index }

    pub fn slave_index(&self) -> u16 { self.slave_index }
}

#[derive(Debug, Clone)]
pub enum CoeError {

    DllFailed { abort_code: u32 },

    InvalidParameter(String),

    InvalidResponse(String),
}

impl std::fmt::Display for CoeError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            Self::DllFailed { abort_code } => {
                if *abort_code != 0 {
                    write!(f, "CoE DLL 失败 (Abort Code 0x{:08X})", abort_code)
                } else {
                    write!(f, "CoE DLL 失败")
                }
            }
            Self::InvalidParameter(msg) => write!(f, "CoE 无效参数: {}", msg),
            Self::InvalidResponse(msg) => write!(f, "CoE 响应异常: {}", msg),
        }
    }
}

impl std::error::Error for CoeError {}

impl From<CoeError> for DarraError {
    fn from(e: CoeError) -> Self {
        DarraError::Other(e.to_string())
    }
}

#[derive(Debug, Clone, Copy, Default)]
pub struct DiagMeta {

    pub max_messages: u8,

    pub newest_message: u8,

    pub newest_acknowledged: u8,

    pub flags: u16,
}

impl DiagMeta {

    pub fn is_ring_buffer(&self) -> bool {
        (self.flags & 0x0010) != 0
    }

    pub fn has_overrun(&self) -> bool {
        (self.flags & 0x0020) != 0
    }
}

impl CoEInstance {

    pub fn poll_has_new_diagnostic(&self) -> i32 {
        let mut abort_code: u32 = 0;
        unsafe {
            ffi::coe_diag_poll_new_available(self.master_index, self.slave_index, &mut abort_code)
        }
    }

    pub fn read_diagnostic_meta(&self) -> std::result::Result<DiagMeta, CoeError> {
        let mut max_msgs: u8 = 0;
        let mut newest: u8 = 0;
        let mut acknowledged: u8 = 0;
        let mut flags: u16 = 0;
        let mut abort_code: u32 = 0;
        let rc = unsafe {
            ffi::coe_diag_read_meta(
                self.master_index,
                self.slave_index,
                &mut max_msgs,
                &mut newest,
                &mut acknowledged,
                &mut flags,
                &mut abort_code,
            )
        };
        if rc == 1 {
            Ok(DiagMeta {
                max_messages: max_msgs,
                newest_message: newest,
                newest_acknowledged: acknowledged,
                flags,
            })
        } else {
            Err(CoeError::DllFailed { abort_code })
        }
    }

    pub fn read_diagnostic_message(
        &self,
        msg_idx: u16,
        buf_size: usize,
    ) -> std::result::Result<Vec<u8>, CoeError> {
        if !(6..=255).contains(&msg_idx) {
            return Err(CoeError::InvalidParameter(format!(
                "msg_idx 必须在 6..255 之间, 实际={}",
                msg_idx
            )));
        }
        if buf_size == 0 || buf_size > 64 * 1024 {
            return Err(CoeError::InvalidParameter(format!(
                "buf_size 必须在 1..65536 之间, 实际={}",
                buf_size
            )));
        }

        let mut buf = vec![0u8; buf_size];
        let mut out_len: c_int = 0;
        let mut abort_code: u32 = 0;
        let rc = unsafe {
            ffi::coe_diag_read_message(
                self.master_index,
                self.slave_index,
                msg_idx as u8,
                buf.as_mut_ptr(),
                buf_size as c_int,
                &mut out_len,
                &mut abort_code,
            )
        };
        if rc == 1 {
            if out_len < 0 || out_len as usize > buf_size {
                return Err(CoeError::InvalidResponse(format!(
                    "DLL 返回 out_len={}, buf_size={}",
                    out_len, buf_size
                )));
            }
            buf.truncate(out_len as usize);
            Ok(buf)
        } else {
            Err(CoeError::DllFailed { abort_code })
        }
    }

    pub fn acknowledge_diagnostic(&self, msg_idx: u16) -> std::result::Result<(), CoeError> {
        if !(6..=255).contains(&msg_idx) {
            return Err(CoeError::InvalidParameter(format!(
                "msg_idx 必须在 6..255 之间, 实际={}",
                msg_idx
            )));
        }
        let mut abort_code: u32 = 0;
        let rc = unsafe {
            ffi::coe_diag_acknowledge(
                self.master_index,
                self.slave_index,
                msg_idx as u8,
                &mut abort_code,
            )
        };
        if rc == 1 {
            Ok(())
        } else {
            Err(CoeError::DllFailed { abort_code })
        }
    }
}

pub struct CaObjectEntry {

    pub sub_index: u8,

    pub data_type: u16,

    pub bit_length: u16,
}

pub fn parse_complete_access_data(data: &[u8], entries: &[CaObjectEntry]) -> Vec<(u8, Vec<u8>)> {
    let mut result = Vec::new();
    if data.is_empty() || entries.is_empty() {
        return result;
    }

    let si0_bytes = 2usize;
    if data.len() >= si0_bytes {
        result.push((0, data[..si0_bytes].to_vec()));
    }
    let mut bit_offset = si0_bytes * 8;

    for i in 1..entries.len() {
        let oe = &entries[i];
        let dt = oe.data_type;
        let is_bit_type = dt >= 0x0030 && dt <= 0x0037;

        let bit_size = if is_bit_type {
            (dt - 0x0030 + 1) as usize
        } else if oe.bit_length > 0 {
            oe.bit_length as usize
        } else {
            8
        };

        if !is_bit_type && (bit_offset % 8) != 0 {
            bit_offset = ((bit_offset + 7) / 8) * 8;
        }

        let byte_start = bit_offset / 8;
        let bytes_needed = (bit_size + 7) / 8;

        if byte_start + bytes_needed <= data.len() {

            result.push((oe.sub_index, data[byte_start..byte_start + bytes_needed].to_vec()));
        }

        bit_offset += bit_size;
    }

    result
}

impl CoEInstance {

    pub fn sdo_read_blocking(
        master_index: u16,
        slave_index: u16,
        index: u16,
        subindex: u8,
        complete_access: bool,
    ) -> std::thread::JoinHandle<Result<Vec<u8>>> {
        std::thread::spawn(move || {
            let coe = CoEInstance::new(master_index, slave_index);
            coe.sdo_read(index, subindex, complete_access)
        })
    }

    pub fn sdo_write_blocking(
        master_index: u16,
        slave_index: u16,
        index: u16,
        subindex: u8,
        complete_access: bool,
        data: Vec<u8>,
    ) -> std::thread::JoinHandle<Result<()>> {
        std::thread::spawn(move || {
            let coe = CoEInstance::new(master_index, slave_index);
            coe.sdo_write(index, subindex, complete_access, &data)
        })
    }
}

#[cfg(feature = "async-tokio")]
impl CoEInstance {

    pub async fn sdo_read_async(
        &self,
        index: u16,
        subindex: u8,
        complete_access: bool,
    ) -> Result<Vec<u8>> {
        let master = self.master_index;
        let slave = self.slave_index;
        tokio::task::spawn_blocking(move || {
            let coe = CoEInstance::new(master, slave);
            coe.sdo_read(index, subindex, complete_access)
        })
        .await
        .map_err(|e| DarraError::Other(format!("tokio join error: {}", e)))?
    }

    pub async fn sdo_write_async(
        &self,
        index: u16,
        subindex: u8,
        complete_access: bool,
        data: Vec<u8>,
    ) -> Result<()> {
        let master = self.master_index;
        let slave = self.slave_index;
        tokio::task::spawn_blocking(move || {
            let coe = CoEInstance::new(master, slave);
            coe.sdo_write(index, subindex, complete_access, &data)
        })
        .await
        .map_err(|e| DarraError::Other(format!("tokio join error: {}", e)))?
    }
}

impl crate::abstractions::MailboxProtocol for CoEInstance {
    fn protocol_type(&self) -> u8 { 0x03 }
    fn protocol_name(&self) -> &'static str { "CoE" }

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

    fn last_error_code(&self) -> u32 {
        self.last_sdo_error.get() as u32
    }

    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, 0x03, &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, 0x03);
        }
    }
}