
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, OnceLock};
use std::time::SystemTime;

pub const SOE_ELEM_DATA_STATE: u8 = 0x01;

pub const SOE_ELEM_NAME: u8 = 0x02;

pub const SOE_ELEM_ATTRIBUTE: u8 = 0x04;

pub const SOE_ELEM_UNIT: u8 = 0x08;

pub const SOE_ELEM_MIN: u8 = 0x10;

pub const SOE_ELEM_MAX: u8 = 0x20;

pub const SOE_ELEM_VALUE: u8 = 0x40;

pub const SOE_ELEM_DEFAULT: u8 = 0x80;

#[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);
    pub const NAME:        Self = Self(0x02);
    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);

    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 }
}

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
}

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)
}

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))
}

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)
}

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

    Binary = 0,

    UInt = 1,

    Int = 2,

    Hexadecimal = 3,

    String = 4,

    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,
        }
    }
}

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

    pub evaluation_factor: u16,

    pub length: u8,

    pub is_list: bool,

    pub is_command: bool,

    pub data_type: SoEDataType,

    pub decimals: u8,

    pub write_protected_preop: bool,

    pub write_protected_safeop: bool,

    pub write_protected_op: bool,
}

impl SoEAttributes {

    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
    }

    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*"
        }
    }
}

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

    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 {

    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)
    }

    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()
    }

    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)
    }
}

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

    pub idn: u16,

    pub name: String,

    pub bit_length: i32,
}

impl ServoMappingEntry {

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

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

    pub at_mapping: Vec<ServoMappingEntry>,

    pub mdt_mapping: Vec<ServoMappingEntry>,

    pub at_bit_size: i32,

    pub mdt_bit_size: i32,
}

impl ServoMappingInfo {

    pub fn at_byte_size(&self) -> i32 { (self.at_bit_size + 7) / 8 }

    pub fn mdt_byte_size(&self) -> i32 { (self.mdt_bit_size + 7) / 8 }
}

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

    pub slave_index: u16,

    pub drive_number: u8,

    pub idn: u16,

    pub new_value: Option<Vec<u8>>,

    pub old_value: Option<Vec<u8>>,
}

impl SoENotificationEventArgs {

    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)
    }
}

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

    pub master_index: u16,

    pub slave_index: u16,

    pub drive_number: u8,

    pub idn: u16,

    pub new_value: Option<Vec<u8>>,

    pub old_value: Option<Vec<u8>>,

    pub timestamp: SystemTime,
}

impl SoENotificationEvent {

    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)
    }
}

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

    pub master_index: u16,

    pub slave_index: u16,

    pub drive_number: u8,

    pub idn: u16,

    pub error_code: SoEErrorCode,

    pub data: Vec<u8>,

    pub timestamp: SystemTime,
}

pub type SoENotificationCallback = Arc<dyn Fn(&SoENotificationEvent) + Send + Sync>;

pub type SoEEmergencyCallback = Arc<dyn Fn(&SoEEmergencyEvent) + Send + Sync>;

#[derive(Default)]
struct SoENativeListeners {
    notification: Vec<SoENotificationCallback>,
    emergency: Vec<SoEEmergencyCallback>,
}

fn soe_listener_registry()
    -> &'static Mutex<HashMap<(u16, u16), SoENativeListeners>>
{
    static REG: OnceLock<Mutex<HashMap<(u16, u16), SoENativeListeners>>> = OnceLock::new();
    REG.get_or_init(|| Mutex::new(HashMap::new()))
}

fn soe_native_registered() -> &'static Mutex<bool> {
    static FLAG: OnceLock<Mutex<bool>> = OnceLock::new();
    FLAG.get_or_init(|| Mutex::new(false))
}

fn soe_error_from_raw(raw: u16) -> SoEErrorCode {
    match raw {
        0x0000 => SoEErrorCode::NoError,
        0x1001 => SoEErrorCode::NoIDN,
        0x1009 => SoEErrorCode::InvalidAccessToElement,
        0x2001 => SoEErrorCode::NoName,
        0x2002 => SoEErrorCode::NameTransmissionTooShort,
        0x2003 => SoEErrorCode::NameTransmissionTooLong,
        0x2004 => SoEErrorCode::NameCannotBeChanged,
        0x2005 => SoEErrorCode::NameWriteProtected,
        0x3002 => SoEErrorCode::AttributeTransmissionTooShort,
        0x3003 => SoEErrorCode::AttributeTransmissionTooLong,
        0x3004 => SoEErrorCode::AttributeCannotBeChanged,
        0x3005 => SoEErrorCode::AttributeWriteProtected,
        0x4001 => SoEErrorCode::NoUnits,
        0x4002 => SoEErrorCode::UnitTransmissionTooShort,
        0x4003 => SoEErrorCode::UnitTransmissionTooLong,
        0x4004 => SoEErrorCode::UnitCannotBeChanged,
        0x4005 => SoEErrorCode::UnitWriteProtected,
        0x5001 => SoEErrorCode::NoMinimumInputValue,
        0x5002 => SoEErrorCode::MinValueTooShort,
        0x5003 => SoEErrorCode::MinValueTooLong,
        0x5004 => SoEErrorCode::MinValueCannotBeChanged,
        0x5005 => SoEErrorCode::MinValueWriteProtected,
        0x6001 => SoEErrorCode::NoMaximumInputValue,
        0x6002 => SoEErrorCode::MaxValueTooShort,
        0x6003 => SoEErrorCode::MaxValueTooLong,
        0x6004 => SoEErrorCode::MaxValueCannotBeChanged,
        0x6005 => SoEErrorCode::MaxValueWriteProtected,
        0x7001 => SoEErrorCode::NoOperationData,
        0x7002 => SoEErrorCode::OpDataTooShort,
        0x7003 => SoEErrorCode::OpDataTooLong,
        0x7004 => SoEErrorCode::OpDataCannotBeChanged,
        0x7005 => SoEErrorCode::OpDataWriteProtected,
        0x7006 => SoEErrorCode::OpDataSmallerThanMin,
        0x7007 => SoEErrorCode::OpDataGreaterThanMax,
        0x800A => SoEErrorCode::InvalidDriveNumber,
        0x800B => SoEErrorCode::GeneralError,
        0x800C => SoEErrorCode::NoElementAddressed,
        _ => SoEErrorCode::Unknown,
    }
}

unsafe extern "C" fn soe_notification_trampoline(
    master_index: u16,
    slave_index: u16,
    drive_no: u8,
    idn: u16,
    _element_flags: u8,
    data: *const std::os::raw::c_void,
    data_len: u16,
) {
    let payload: Option<Vec<u8>> = if data.is_null() || data_len == 0 {
        None
    } else {
        Some(unsafe { std::slice::from_raw_parts(data as *const u8, data_len as usize) }.to_vec())
    };
    let snapshot: Vec<SoENotificationCallback> = match soe_listener_registry().lock() {
        Ok(g) => g.get(&(master_index, slave_index)).map(|l| l.notification.clone()).unwrap_or_default(),
        Err(p) => p.into_inner().get(&(master_index, slave_index)).map(|l| l.notification.clone()).unwrap_or_default(),
    };
    if snapshot.is_empty() {
        return;
    }
    let ev = SoENotificationEvent {
        master_index,
        slave_index,
        drive_number: drive_no,
        idn,
        new_value: payload,
        old_value: None,
        timestamp: SystemTime::now(),
    };
    for cb in &snapshot {
        let _ = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| cb(&ev)));
    }
}

extern "C" fn soe_emergency_trampoline(
    master_index: u16,
    slave_index: u16,
    drive_no: u8,
    error_code: u16,
) {
    let snapshot: Vec<SoEEmergencyCallback> = match soe_listener_registry().lock() {
        Ok(g) => g.get(&(master_index, slave_index)).map(|l| l.emergency.clone()).unwrap_or_default(),
        Err(p) => p.into_inner().get(&(master_index, slave_index)).map(|l| l.emergency.clone()).unwrap_or_default(),
    };
    if snapshot.is_empty() {
        return;
    }
    let ev = SoEEmergencyEvent {
        master_index,
        slave_index,
        drive_number: drive_no,
        idn: 0,
        error_code: soe_error_from_raw(error_code),
        data: Vec::new(),
        timestamp: SystemTime::now(),
    };
    for cb in &snapshot {
        let _ = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| cb(&ev)));
    }
}

fn ensure_soe_native_registered() {
    let mut registered = match soe_native_registered().lock() {
        Ok(g) => g,
        Err(p) => p.into_inner(),
    };
    if !*registered {
        unsafe {
            ffi::RegisterSoENotificationCallback(soe_notification_trampoline);
            ffi::RegisterSoEEmergencyCallback(soe_emergency_trampoline);
        }
        *registered = true;
    }
}

pub mod standard_idn {

    pub const IDN_LIST: u16 = 0x0000;

    pub const AT_CONFIG: u16 = 0x0010;

    pub const MDT_CONFIG: u16 = 0x0018;

    pub const CYCLE_TIME: u16 = 0x0001;

    pub const NOTIFICATION_REQUEST: u16 = 0x007F;
}

pub struct SoEInstance {
    master_index: u16,
    slave_index: u16,
    drive_number: u8,

    monitored_idns: Mutex<HashMap<u16, Option<Vec<u8>>>>,

    last_soe_error: Option<SoEErrorCode>,

    notification_callbacks: Arc<Mutex<Vec<SoENotificationCallback>>>,

    emergency_callbacks: Arc<Mutex<Vec<SoEEmergencyCallback>>>,
}

impl SoEInstance {

    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 }

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

    pub fn last_soe_error(&self) -> Option<SoEErrorCode> { self.last_soe_error }

    pub fn add_notification_callback(&self, cb: SoENotificationCallback) {
        if let Ok(mut list) = self.notification_callbacks.lock() {
            list.push(cb);
        }
    }

    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));
        }
    }

    pub fn add_emergency_callback(&self, cb: SoEEmergencyCallback) {
        if let Ok(mut list) = self.emergency_callbacks.lock() {
            list.push(cb);
        }
    }

    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));
        }
    }

    pub fn register_native_notification_callback(&self, cb: SoENotificationCallback) {
        ensure_soe_native_registered();
        let mut reg = match soe_listener_registry().lock() {
            Ok(g) => g,
            Err(p) => p.into_inner(),
        };
        reg.entry((self.master_index, self.slave_index))
            .or_default()
            .notification
            .push(cb);
    }

    pub fn unregister_native_notification_callback(&self, cb: &SoENotificationCallback) {
        if let Ok(mut reg) = soe_listener_registry().lock() {
            if let Some(listeners) = reg.get_mut(&(self.master_index, self.slave_index)) {
                listeners.notification.retain(|c| !Arc::ptr_eq(c, cb));
            }
        }
    }

    pub fn register_native_emergency_callback(&self, cb: SoEEmergencyCallback) {
        ensure_soe_native_registered();
        let mut reg = match soe_listener_registry().lock() {
            Ok(g) => g,
            Err(p) => p.into_inner(),
        };
        reg.entry((self.master_index, self.slave_index))
            .or_default()
            .emergency
            .push(cb);
    }

    pub fn unregister_native_emergency_callback(&self, cb: &SoEEmergencyCallback) {
        if let Ok(mut reg) = soe_listener_registry().lock() {
            if let Some(listeners) = reg.get_mut(&(self.master_index, self.slave_index)) {
                listeners.emergency.retain(|c| !Arc::ptr_eq(c, cb));
            }
        }
    }

    pub fn clear_native_callbacks(&self) {
        if let Ok(mut reg) = soe_listener_registry().lock() {
            reg.remove(&(self.master_index, self.slave_index));
        }
    }

    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)));
        }
    }

    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)));
        }
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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)
    }

    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]))
    }

    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]))
    }

    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))
    }

    pub fn read_min_value(&self, idn: u16, timeout_ms: i32) -> Result<Vec<u8>> {
        self.read(idn, SOE_ELEM_MIN, timeout_ms)
    }

    pub fn read_max_value(&self, idn: u16, timeout_ms: i32) -> Result<Vec<u8>> {
        self.read(idn, SOE_ELEM_MAX, timeout_ms)
    }

    pub fn read_default_value(&self, idn: u16, timeout_ms: i32) -> Result<Vec<u8>> {
        self.read(idn, SOE_ELEM_DEFAULT, timeout_ms)
    }

    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]]))
    }

    pub fn execute_command(&self, idn: u16, timeout_ms: i32, poll_interval_ms: i32) -> Result<()> {

        let current = self.read_data_state(idn, timeout_ms)?;

        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));
                }
                if (state & 0x0001) == 0 {
                    return Ok(());
                }
            }
        }
        Err(DarraError::Timeout)
    }

    pub fn get_available_idns(&self, timeout_ms: i32) -> Result<Vec<u16>> {
        self.available_idns(timeout_ms)
    }

    pub fn get_parameter_info(&self, idn: u16, timeout_ms: i32) -> SoEParameter {
        self.parameter_info(idn, timeout_ms)
    }

    pub fn get_idn_mapping(&self, timeout_ms: i32) -> ServoMappingInfo {
        self.idn_mapping(timeout_ms)
    }

    pub fn get_monitored_idns(&self) -> Vec<u16> {
        self.monitored_idns()
    }

    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
    }

    pub fn read_parameter(&self, idn: u16, timeout_ms: i32) -> SoEParameter {
        self.parameter_info(idn, timeout_ms)
    }

    pub fn read_all_parameters(&self, timeout_ms: i32) -> Vec<SoEParameter> {
        let standard_idns: &[u16] = &[
            0x0000,
            0x0001,
            0x0002,
            0x0003,
            0x0004,
            0x0010,
            0x0011,
            0x0012,
            0x0013,
            0x0014,
            0x0015,
            0x0016,
            0x0021,
            0x0024,
            0x0032,
            0x0033,
            0x0040,
            0x0047,
        ];

        let mut params = Vec::new();
        for &idn in standard_idns {

            let param = self.parameter_info(idn, timeout_ms);

            if !param.name.is_empty() || !param.value.is_empty() {
                params.push(param);
            }
        }
        params
    }

    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)
    }

    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)
    }

    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
    }

    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,
        };

        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);
        }

        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 {

                    if decoded.length < 3 {
                        entry.bit_length = 16 << decoded.length;
                        *total_bits += entry.bit_length;
                    }
                }
            }

            if let Ok(name) = self.read_name(mapped_idn, 500) {
                entry.name = name;
            }

            entries.push(entry);
        }
    }

    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
    }

    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();
        }
    }

    pub fn monitored_idns(&self) -> Vec<u16> {
        self.monitored_idns.lock().map(|m| m.keys().copied().collect()).unwrap_or_default()
    }

    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
    }
}

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]]))
}

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]]))
}

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]]))
}

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]]))
}

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))
}

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))
}

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]))
}

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)
}

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)
}

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)
}

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)
}

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)
}

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)
}

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)
}

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)) }
}

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)) }
}

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)
}

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(" ")
}

pub fn format_byte_array_typed(data: &[u8], data_type: u8) -> String {
    if data.is_empty() {
        return String::new();
    }
    match data_type {
        1 => {
            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 => {
            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 => {
            format!("0x{}", data.iter().map(|b| format!("{:02X}", b)).collect::<String>())
        }
        4 => {

            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 => {
            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),
    }
}

impl SoEInstance {

    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)
        })
    }

    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)
        })
    }

    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)
        })
    }
}

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

    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)))?
    }

    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)))?
    }

    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)))?
    }
}

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);
        }
    }
}
