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

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

    pub od_index: u16,

    pub sub_index: u8,

    pub bit_length: u8,

    pub byte_offset: u32,

    pub bit_offset: u8,

    pub is_output: bool,
}

impl PdoMapEntry {

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

pub struct Cia402PdoMap {

    master_index: u16,

    slave_index: u16,

    pub(crate) output_map: HashMap<(u16, u8), PdoMapEntry>,

    pub(crate) input_map: HashMap<(u16, u8), PdoMapEntry>,

    out_ptr: *mut u8,

    out_size: i32,

    in_ptr: *mut u8,

    in_size: i32,
}

unsafe impl Send for Cia402PdoMap {}
unsafe impl Sync for Cia402PdoMap {}

impl Cia402PdoMap {

    pub fn scan(master_index: u16, slave_index: u16) -> Result<Self> {

        let mut out_size: c_int = 0;
        let mut out_ptr: *mut u8 = std::ptr::null_mut();
        let mut in_size: c_int = 0;
        let mut in_ptr: *mut u8 = std::ptr::null_mut();
        let io_ok = unsafe {
            ffi::GetIO(master_index, slave_index,
                       &mut out_size, &mut out_ptr,
                       &mut in_size, &mut in_ptr)
        };
        if io_ok == 0 {
            return Err(DarraError::PdoFailed("获取从站 IOmap 失败".into()));
        }

        let mut output_map = HashMap::new();
        let mut input_map = HashMap::new();

        let rxpdo_count = Self::read_u8_sdo(master_index, slave_index, 0x1C12, 0)
            .unwrap_or(0);

        let mut out_byte_offset: u32 = 0;
        for sm_idx in 1..=rxpdo_count {

            let pdo_idx = match Self::read_u16_sdo(master_index, slave_index, 0x1C12, sm_idx) {
                Ok(v) => v,
                Err(_) => continue,
            };

            let map_count = Self::read_u8_sdo(master_index, slave_index, pdo_idx, 0)
                .unwrap_or(0);
            for map_sub in 1..=map_count {
                let map_val = match Self::read_u32_sdo(master_index, slave_index, pdo_idx, map_sub) {
                    Ok(v) => v,
                    Err(_) => continue,
                };

                let obj_idx   = ((map_val >> 16) & 0xFFFF) as u16;
                let obj_sub   = ((map_val >> 8)  & 0xFF) as u8;
                let bit_len   = (map_val & 0xFF) as u8;
                if obj_idx == 0 || bit_len == 0 { continue; }
                let entry = PdoMapEntry {
                    od_index:    obj_idx,
                    sub_index:   obj_sub,
                    bit_length:  bit_len,
                    byte_offset: out_byte_offset,
                    bit_offset:  0,
                    is_output:   true,
                };
                out_byte_offset += (bit_len as u32 + 7) / 8;
                output_map.insert((obj_idx, obj_sub), entry);
            }
        }

        let txpdo_count = Self::read_u8_sdo(master_index, slave_index, 0x1C13, 0)
            .unwrap_or(0);

        let mut in_byte_offset: u32 = 0;
        for sm_idx in 1..=txpdo_count {
            let pdo_idx = match Self::read_u16_sdo(master_index, slave_index, 0x1C13, sm_idx) {
                Ok(v) => v,
                Err(_) => continue,
            };
            let map_count = Self::read_u8_sdo(master_index, slave_index, pdo_idx, 0)
                .unwrap_or(0);
            for map_sub in 1..=map_count {
                let map_val = match Self::read_u32_sdo(master_index, slave_index, pdo_idx, map_sub) {
                    Ok(v) => v,
                    Err(_) => continue,
                };
                let obj_idx   = ((map_val >> 16) & 0xFFFF) as u16;
                let obj_sub   = ((map_val >> 8)  & 0xFF) as u8;
                let bit_len   = (map_val & 0xFF) as u8;
                if obj_idx == 0 || bit_len == 0 { continue; }
                let entry = PdoMapEntry {
                    od_index:    obj_idx,
                    sub_index:   obj_sub,
                    bit_length:  bit_len,
                    byte_offset: in_byte_offset,
                    bit_offset:  0,
                    is_output:   false,
                };
                in_byte_offset += (bit_len as u32 + 7) / 8;
                input_map.insert((obj_idx, obj_sub), entry);
            }
        }

        Ok(Self {
            master_index,
            slave_index,
            output_map,
            input_map,
            out_ptr,
            out_size,
            in_ptr,
            in_size,
        })
    }

    pub fn read_u32(&self, od_index: u16, sub_index: u8) -> Result<u32> {
        if let Some(entry) = self.input_map.get(&(od_index, sub_index)) {

            let off = entry.byte_offset as usize;
            let sz  = entry.byte_size() as usize;
            if !self.in_ptr.is_null() && off + sz <= self.in_size as usize {
                let val = unsafe {
                    let ptr = self.in_ptr.add(off);
                    match sz {
                        1 => *ptr as u32,
                        2 => u16::from_le_bytes([*ptr, *ptr.add(1)]) as u32,
                        4 => u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)]),
                        _ => 0,
                    }
                };
                return Ok(val);
            }
        }

        Self::read_u32_sdo(self.master_index, self.slave_index, od_index, sub_index)
    }

    pub fn write_u32(&self, od_index: u16, sub_index: u8, value: u32) -> Result<()> {
        if let Some(entry) = self.output_map.get(&(od_index, sub_index)) {
            let off = entry.byte_offset as usize;
            let sz  = entry.byte_size() as usize;
            if !self.out_ptr.is_null() && off + sz <= self.out_size as usize {
                unsafe {
                    let ptr = self.out_ptr.add(off);
                    let bytes = value.to_le_bytes();
                    std::ptr::copy_nonoverlapping(bytes.as_ptr(), ptr, sz.min(4));
                }
                return Ok(());
            }
        }

        let bytes = value.to_le_bytes();
        let ok = unsafe {
            ffi::SDOwrite_raw(self.master_index, self.slave_index, od_index, sub_index,
                              0, bytes.as_ptr(), 4)
        };
        if ok != 0 { Ok(()) } else { Err(DarraError::SdoWriteFailed { index: od_index, subindex: sub_index, abort_code: None }) }
    }

    pub fn read_u16(&self, od_index: u16, sub_index: u8) -> Result<u16> {
        Ok(self.read_u32(od_index, sub_index)? as u16)
    }

    pub fn write_u16(&self, od_index: u16, sub_index: u8, value: u16) -> Result<()> {
        if let Some(entry) = self.output_map.get(&(od_index, sub_index)) {
            let off = entry.byte_offset as usize;
            if !self.out_ptr.is_null() && off + 2 <= self.out_size as usize {
                unsafe {
                    let bytes = value.to_le_bytes();
                    std::ptr::copy_nonoverlapping(bytes.as_ptr(), self.out_ptr.add(off), 2);
                }
                return Ok(());
            }
        }
        let bytes = value.to_le_bytes();
        let ok = unsafe {
            ffi::SDOwrite_raw(self.master_index, self.slave_index, od_index, sub_index,
                              0, bytes.as_ptr(), 2)
        };
        if ok != 0 { Ok(()) } else { Err(DarraError::SdoWriteFailed { index: od_index, subindex: sub_index, abort_code: None }) }
    }

    pub fn output_entries(&self) -> impl Iterator<Item = &PdoMapEntry> {
        self.output_map.values()
    }

    pub fn input_entries(&self) -> impl Iterator<Item = &PdoMapEntry> {
        self.input_map.values()
    }

    pub fn is_output_mapped(&self, od_index: u16, sub_index: u8) -> bool {
        self.output_map.contains_key(&(od_index, sub_index))
    }

    pub fn is_input_mapped(&self, od_index: u16, sub_index: u8) -> bool {
        self.input_map.contains_key(&(od_index, sub_index))
    }

    fn read_u8_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u8> {
        let mut size: c_int = 0;
        let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
        if ptr.is_null() || size < 1 {
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
        }
        let val = unsafe { *ptr };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(val)
    }

    fn read_u16_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u16> {
        let mut size: c_int = 0;
        let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
        if ptr.is_null() || size < 2 {
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
        }
        let val = unsafe { u16::from_le_bytes([*ptr, *ptr.add(1)]) };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(val)
    }

    fn read_u32_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u32> {
        let mut size: c_int = 0;
        let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
        if ptr.is_null() || size < 4 {
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
        }
        let val = unsafe {
            u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)])
        };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(val)
    }
}

impl std::fmt::Debug for Cia402PdoMap {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("Cia402PdoMap")
            .field("master_index", &self.master_index)
            .field("slave_index", &self.slave_index)
            .field("output_entries", &self.output_map.len())
            .field("input_entries", &self.input_map.len())
            .finish()
    }
}

pub const OD_CONTROLWORD: u16 = 0x6040;

pub const OD_STATUSWORD: u16 = 0x6041;

pub const OD_MODES_OF_OPERATION: u16 = 0x6060;

pub const OD_MODES_OF_OPERATION_DISPLAY: u16 = 0x6061;

pub const OD_TARGET_POSITION: u16 = 0x607A;

pub const OD_POSITION_ACTUAL: u16 = 0x6064;

pub const OD_TARGET_VELOCITY: u16 = 0x60FF;

pub const OD_VELOCITY_ACTUAL: u16 = 0x606C;

pub const OD_TARGET_TORQUE: u16 = 0x6071;

pub const OD_TORQUE_ACTUAL: u16 = 0x6077;

pub const OD_MAX_PROFILE_VELOCITY: u16 = 0x6080;

pub const OD_PROFILE_VELOCITY: u16 = 0x6081;

pub const OD_PROFILE_ACCELERATION: u16 = 0x6083;

pub const OD_PROFILE_DECELERATION: u16 = 0x6084;

pub const OD_QUICK_STOP_DECELERATION: u16 = 0x6085;

pub const OD_HOMING_METHOD: u16 = 0x6098;

pub const OD_HOMING_SPEEDS: u16 = 0x6099;

pub const OD_HOME_OFFSET: u16 = 0x607C;

pub const OD_TORQUE_OFFSET: u16 = 0x60B2;

pub const OD_SOFTWARE_POSITION_LIMIT: u16 = 0x607D;

pub const OD_SUPPORTED_DRIVE_MODES: u16 = 0x6502;

pub const OD_POLARITY: u16 = 0x607E;

pub const OD_MOTION_PROFILE_TYPE: u16 = 0x6086;

pub const OD_TOUCH_PROBE_FUNCTION: u16 = 0x60B8;

pub const OD_TOUCH_PROBE_STATUS: u16 = 0x60B9;

pub const OD_TOUCH_PROBE_POS_EDGE: u16 = 0x60BA;

pub const OD_TOUCH_PROBE_NEG_EDGE: u16 = 0x60BB;

pub const OD_HOMING_ACCELERATION: u16 = 0x609A;

pub const OD_DIGITAL_INPUTS: u16 = 0x60FD;

pub const OD_DIGITAL_OUTPUTS: u16 = 0x60FE;

pub const OD_MAX_TORQUE: u16 = 0x6072;

pub const OD_MOTOR_RATED_TORQUE: u16 = 0x6076;

pub const OD_POSITION_OFFSET: u16 = 0x60B0;

pub const OD_VELOCITY_OFFSET: u16 = 0x60B1;

pub const OD_INTERPOLATION_TIME_PERIOD: u16 = 0x60C2;

pub const OD_QUICK_STOP_OPTION_CODE: u16 = 0x605A;

pub const OD_POSITIVE_TORQUE_LIMIT: u16 = 0x60E0;

pub const OD_NEGATIVE_TORQUE_LIMIT: u16 = 0x60E1;

pub const OD_SUPPORTED_HOMING_METHODS: u16 = 0x60E3;

pub const OD_TXPDO_DATA_INVALID: u16 = 0x603E;

pub const OD_SYNCHRONIZATION_SETTINGS: u16 = 0x60D9;

pub const OD_DRIVE_SYNC_STATUS: u16 = 0x60DA;

pub const OD_ERROR_CODE: u16 = 0x603F;

pub const OD_DRIVE_DATA: u16 = 0x6510;

pub const CW_SHUTDOWN: u16 = 0x06;

pub const CW_SWITCH_ON: u16 = 0x07;

pub const CW_ENABLE_OPERATION: u16 = 0x0F;

pub const CW_DISABLE_VOLTAGE: u16 = 0x00;

pub const CW_QUICK_STOP: u16 = 0x02;

pub const CW_FAULT_RESET: u16 = 0x80;

pub const CW_HALT: u16 = 0x0100;

pub const SW_READY_TO_SWITCH_ON: u16 = 0x0001;

pub const SW_SWITCHED_ON: u16 = 0x0002;

pub const SW_OPERATION_ENABLED: u16 = 0x0004;

pub const SW_FAULT: u16 = 0x0008;

pub const SW_VOLTAGE_ENABLED: u16 = 0x0010;

pub const SW_QUICK_STOP: u16 = 0x0020;

pub const SW_SWITCH_ON_DISABLED: u16 = 0x0040;

pub const SW_WARNING: u16 = 0x0080;

pub const SW_REMOTE: u16 = 0x0200;

pub const SW_TARGET_REACHED: u16 = 0x0400;

pub const SW_INTERNAL_LIMIT: u16 = 0x0800;

pub const SW_OP_MODE_SPECIFIC_1: u16 = 0x1000;

pub const SW_OP_MODE_SPECIFIC_2: u16 = 0x2000;

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

    NotReadyToSwitchOn = 0,

    SwitchOnDisabled = 1,

    ReadyToSwitchOn = 2,

    SwitchedOn = 3,

    OperationEnabled = 4,

    QuickStopActive = 5,

    FaultReactionActive = 6,

    Fault = 7,

    Unknown = 99,
}

impl StateCiA402 {

    pub fn from_statusword(sw: u16) -> Self {
        if (sw & SW_FAULT) != 0 {
            if (sw & 0x0F) == 0x0F {
                return Self::FaultReactionActive;
            }
            return Self::Fault;
        }
        let mask = sw & 0x006F;
        match mask {
            0x0000 => Self::NotReadyToSwitchOn,
            0x0040 => Self::SwitchOnDisabled,
            0x0021 => Self::ReadyToSwitchOn,
            0x0023 => Self::SwitchedOn,
            0x0027 => Self::OperationEnabled,
            0x0007 => Self::QuickStopActive,
            _ => Self::Unknown,
        }
    }

    pub fn description(&self) -> &'static str {
        match self {
            Self::NotReadyToSwitchOn => "初始化中",
            Self::SwitchOnDisabled => "驱动禁用",
            Self::ReadyToSwitchOn => "准备就绪",
            Self::SwitchedOn => "已开启",
            Self::OperationEnabled => "运行使能",
            Self::QuickStopActive => "快速停止",
            Self::FaultReactionActive => "故障反应中",
            Self::Fault => "故障",
            Self::Unknown => "未知",
        }
    }
}

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

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

    PP = 1,

    VL = 2,

    PV = 3,

    PT = 4,

    HM = 6,

    IP = 7,

    CSP = 8,

    CSV = 9,

    CST = 10,

    CSTCA = 11,
}

impl ModeCiA402 {

    pub fn supported_bit(&self) -> Option<u32> {
        match self {
            Self::PP => Some(0),
            Self::VL => Some(1),
            Self::PV => Some(2),
            Self::PT => Some(3),
            Self::HM => Some(5),
            Self::IP => Some(6),
            Self::CSP => Some(7),
            Self::CSV => Some(8),
            Self::CST => Some(9),
            Self::CSTCA => Some(10),
        }
    }

    pub fn description(&self) -> &'static str {
        match self {
            Self::PP => "轮廓位置 (PP)",
            Self::VL => "速度 (VL)",
            Self::PV => "轮廓速度 (PV)",
            Self::PT => "轮廓转矩 (PT)",
            Self::HM => "回零 (HM)",
            Self::IP => "插补位置 (IP)",
            Self::CSP => "周期同步位置 (CSP)",
            Self::CSV => "周期同步速度 (CSV)",
            Self::CST => "周期同步转矩 (CST)",
            Self::CSTCA => "周期同步转矩加速度 (CSTCA)",
        }
    }
}

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

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

    NotAttempted = 0,

    Success = 1,

    NotSupported = 2,

    ReadFailed = 3,
}

impl Cia402ReadStatus {

    pub fn classify_abort(abort_code: u32) -> Self {
        match SDOError::from_u32(abort_code) {
            SDOError::ObjectDoesNotExist | SDOError::SubindexDoesNotExist => Self::NotSupported,
            _ => Self::ReadFailed,
        }
    }

    pub fn description(&self) -> &'static str {
        match self {
            Self::NotAttempted => "未读取",
            Self::Success => "读取成功",
            Self::NotSupported => "从站不支持",
            Self::ReadFailed => "读取失败",
        }
    }
}

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

#[derive(Debug, Clone, Copy)]
pub struct Cia402ReadResult<T> {

    pub value: T,

    pub status: Cia402ReadStatus,

    pub abort_code: u32,
}

impl<T> Cia402ReadResult<T> {

    pub fn new(value: T, status: Cia402ReadStatus, abort_code: u32) -> Self {
        Self { value, status, abort_code }
    }

    pub fn ok(&self) -> bool {
        self.status == Cia402ReadStatus::Success
    }

    pub fn not_supported(&self) -> bool {
        self.status == Cia402ReadStatus::NotSupported
    }
}

pub trait Cia402Readable: Copy + Default {

    fn from_le_slice(data: &[u8]) -> Self;
}

macro_rules! impl_cia402_readable {
    ($($t:ty),+ $(,)?) => {$(
        impl Cia402Readable for $t {
            fn from_le_slice(data: &[u8]) -> Self {
                const N: usize = std::mem::size_of::<$t>();
                let mut buf = [0u8; N];
                let n = data.len().min(N);
                buf[..n].copy_from_slice(&data[..n]);
                <$t>::from_le_bytes(buf)
            }
        }
    )+};
}
impl_cia402_readable!(u8, i8, u16, i16, u32, i32, u64, i64);

pub struct CiA402Instance {

    master_index: u16,

    slave_index: u16,

    pdo_map: Cia402PdoMap,
}

impl CiA402Instance {

    pub fn new(master_index: u16, slave_index: u16, pdo_map: Cia402PdoMap) -> Self {
        Self { master_index, slave_index, pdo_map }
    }

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

    pub fn pdo_map(&self) -> &Cia402PdoMap {
        &self.pdo_map
    }

    pub fn initialize_pdo_offsets(&mut self) -> Result<()> {
        self.pdo_map = Cia402PdoMap::scan(self.master_index, self.slave_index)?;
        Ok(())
    }

    pub fn reset_pdo_offsets(&mut self) {
        self.pdo_map.output_map.clear();
        self.pdo_map.input_map.clear();
    }

    fn sdo_read_u16(&self, index: u16, sub: u8) -> u16 {
        self.pdo_map.read_u16(index, sub).unwrap_or(0)
    }

    fn sdo_read_i32(&self, index: u16, sub: u8) -> i32 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0) as i32
    }

    fn sdo_read_u32(&self, index: u16, sub: u8) -> u32 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0)
    }

    fn sdo_read_u8(&self, index: u16, sub: u8) -> u8 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0) as u8
    }

    fn sdo_read_i8(&self, index: u16, sub: u8) -> i8 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0) as i8
    }

    fn sdo_read_i16(&self, index: u16, sub: u8) -> i16 {
        self.pdo_map.read_u16(index, sub).unwrap_or(0) as i16
    }

    fn sdo_write_u16(&self, index: u16, sub: u8, val: u16) {
        let _ = self.pdo_map.write_u16(index, sub, val);
    }

    fn sdo_write_u32(&self, index: u16, sub: u8, val: u32) {
        let _ = self.pdo_map.write_u32(index, sub, val);
    }

    fn sdo_write_i32(&self, index: u16, sub: u8, val: i32) {
        let _ = self.pdo_map.write_u32(index, sub, val as u32);
    }

    fn sdo_write_u8(&self, index: u16, sub: u8, val: u8) {

        let bytes = [val];
        unsafe {
            ffi::SDOwrite_raw(self.master_index, self.slave_index, index, sub,
                              0, bytes.as_ptr(), 1);
        }
    }

    fn sdo_write_i8(&self, index: u16, sub: u8, val: i8) {
        self.sdo_write_u8(index, sub, val as u8);
    }

    fn sdo_write_i16(&self, index: u16, sub: u8, val: i16) {
        self.sdo_write_u16(index, sub, val as u16);
    }

    pub fn statusword(&self) -> u16 {
        self.sdo_read_u16(OD_STATUSWORD, 0)
    }

    pub fn controlword(&self) -> u16 {
        self.sdo_read_u16(OD_CONTROLWORD, 0)
    }

    pub fn set_controlword(&self, value: u16) {
        self.sdo_write_u16(OD_CONTROLWORD, 0, value);
    }

    pub fn state_drive(&self) -> StateCiA402 {
        StateCiA402::from_statusword(self.statusword())
    }

    pub fn target_reached(&self) -> bool {
        (self.statusword() & SW_TARGET_REACHED) != 0
    }

    pub fn has_fault(&self) -> bool {
        (self.statusword() & SW_FAULT) != 0
    }

    pub fn has_warning(&self) -> bool {
        (self.statusword() & SW_WARNING) != 0
    }

    pub fn is_remote(&self) -> bool {
        (self.statusword() & SW_REMOTE) != 0
    }

    pub fn operation_mode(&self) -> i8 {
        self.sdo_read_i8(OD_MODES_OF_OPERATION_DISPLAY, 0)
    }

    pub fn set_operation_mode(&self, mode: ModeCiA402) {
        self.sdo_write_i8(OD_MODES_OF_OPERATION, 0, mode as i8);
    }

    pub fn enable(&self, max_retries: i32) -> bool {

        unsafe { ffi::CiA402_Enable(self.master_index, self.slave_index, max_retries) != 0 }
    }

    pub fn enable_default(&self) -> bool {
        self.enable(10)
    }

    pub fn disable_operation(&self) {
        self.set_controlword(CW_SWITCH_ON);
    }

    pub fn disable(&self) {
        self.set_controlword(CW_DISABLE_VOLTAGE);
    }

    pub fn quick_stop(&self) {
        self.set_controlword(CW_QUICK_STOP);
    }

    pub fn fault_reset(&self) {

        self.set_controlword(0x00);

        std::thread::sleep(std::time::Duration::from_millis(1));

        self.set_controlword(CW_FAULT_RESET);
    }

    pub fn position_actual(&self) -> i32 {
        self.sdo_read_i32(OD_POSITION_ACTUAL, 0)
    }

    pub fn velocity_actual(&self) -> i32 {
        self.sdo_read_i32(OD_VELOCITY_ACTUAL, 0)
    }

    pub fn torque_actual(&self) -> i16 {
        self.sdo_read_i16(OD_TORQUE_ACTUAL, 0)
    }

    pub fn set_target_position(&self, value: i32) {
        self.sdo_write_i32(OD_TARGET_POSITION, 0, value);
    }

    pub fn target_position(&self) -> i32 {
        self.sdo_read_i32(OD_TARGET_POSITION, 0)
    }

    pub fn set_target_velocity(&self, value: i32) {
        self.sdo_write_i32(OD_TARGET_VELOCITY, 0, value);
    }

    pub fn target_velocity(&self) -> i32 {
        self.sdo_read_i32(OD_TARGET_VELOCITY, 0)
    }

    pub fn set_target_torque(&self, value: i16) {
        self.sdo_write_i16(OD_TARGET_TORQUE, 0, value);
    }

    pub fn target_torque(&self) -> i16 {
        self.sdo_read_i16(OD_TARGET_TORQUE, 0)
    }

    pub fn profile_velocity(&self) -> u32 {
        self.sdo_read_u32(OD_PROFILE_VELOCITY, 0)
    }

    pub fn set_profile_velocity(&self, value: u32) {
        self.sdo_write_u32(OD_PROFILE_VELOCITY, 0, value);
    }

    pub fn profile_acceleration(&self) -> u32 {
        self.sdo_read_u32(OD_PROFILE_ACCELERATION, 0)
    }

    pub fn set_profile_acceleration(&self, value: u32) {
        self.sdo_write_u32(OD_PROFILE_ACCELERATION, 0, value);
    }

    pub fn profile_deceleration(&self) -> u32 {
        self.sdo_read_u32(OD_PROFILE_DECELERATION, 0)
    }

    pub fn set_profile_deceleration(&self, value: u32) {
        self.sdo_write_u32(OD_PROFILE_DECELERATION, 0, value);
    }

    pub fn quick_stop_deceleration(&self) -> u32 {
        self.sdo_read_u32(OD_QUICK_STOP_DECELERATION, 0)
    }

    pub fn set_quick_stop_deceleration(&self, value: u32) {
        self.sdo_write_u32(OD_QUICK_STOP_DECELERATION, 0, value);
    }

    pub fn polarity(&self) -> u8 {
        self.sdo_read_u8(OD_POLARITY, 0)
    }

    pub fn set_polarity(&self, value: u8) {
        self.sdo_write_u8(OD_POLARITY, 0, value);
    }

    pub fn motion_profile_type(&self) -> i16 {
        self.sdo_read_i16(OD_MOTION_PROFILE_TYPE, 0)
    }

    pub fn set_motion_profile_type(&self, value: i16) {
        self.sdo_write_i16(OD_MOTION_PROFILE_TYPE, 0, value);
    }

    pub fn software_position_limit_min(&self) -> i32 {
        self.sdo_read_i32(OD_SOFTWARE_POSITION_LIMIT, 1)
    }

    pub fn set_software_position_limit_min(&self, value: i32) {
        self.sdo_write_i32(OD_SOFTWARE_POSITION_LIMIT, 1, value);
    }

    pub fn software_position_limit_max(&self) -> i32 {
        self.sdo_read_i32(OD_SOFTWARE_POSITION_LIMIT, 2)
    }

    pub fn set_software_position_limit_max(&self, value: i32) {
        self.sdo_write_i32(OD_SOFTWARE_POSITION_LIMIT, 2, value);
    }

    pub fn home_offset(&self) -> i32 {
        self.sdo_read_i32(OD_HOME_OFFSET, 0)
    }

    pub fn set_home_offset(&self, value: i32) {
        self.sdo_write_i32(OD_HOME_OFFSET, 0, value);
    }

    pub fn homing_speed_search(&self) -> u32 {
        self.sdo_read_u32(OD_HOMING_SPEEDS, 1)
    }

    pub fn set_homing_speed_search(&self, value: u32) {
        self.sdo_write_u32(OD_HOMING_SPEEDS, 1, value);
    }

    pub fn homing_speed_zero(&self) -> u32 {
        self.sdo_read_u32(OD_HOMING_SPEEDS, 2)
    }

    pub fn set_homing_speed_zero(&self, value: u32) {
        self.sdo_write_u32(OD_HOMING_SPEEDS, 2, value);
    }

    pub fn homing_acceleration(&self) -> u32 {
        self.sdo_read_u32(OD_HOMING_ACCELERATION, 0)
    }

    pub fn set_homing_acceleration(&self, value: u32) {
        self.sdo_write_u32(OD_HOMING_ACCELERATION, 0, value);
    }

    pub fn configure_touch_probe(&self, function: u16) {
        self.sdo_write_u16(OD_TOUCH_PROBE_FUNCTION, 0, function);
    }

    pub fn touch_probe_status(&self) -> u16 {
        self.sdo_read_u16(OD_TOUCH_PROBE_STATUS, 0)
    }

    pub fn touch_probe_positive_edge(&self) -> i32 {
        self.sdo_read_i32(OD_TOUCH_PROBE_POS_EDGE, 0)
    }

    pub fn touch_probe_negative_edge(&self) -> i32 {
        self.sdo_read_i32(OD_TOUCH_PROBE_NEG_EDGE, 0)
    }

    pub fn digital_inputs(&self) -> u32 {
        self.sdo_read_u32(OD_DIGITAL_INPUTS, 0)
    }

    pub fn digital_outputs(&self) -> u32 {
        self.sdo_read_u32(OD_DIGITAL_OUTPUTS, 1)
    }

    pub fn set_digital_outputs(&self, value: u32) {
        self.sdo_write_u32(OD_DIGITAL_OUTPUTS, 1, value);
    }

    pub fn max_torque(&self) -> u16 {
        self.sdo_read_u16(OD_MAX_TORQUE, 0)
    }

    pub fn set_max_torque(&self, value: u16) {
        self.sdo_write_u16(OD_MAX_TORQUE, 0, value);
    }

    pub fn motor_rated_torque(&self) -> u32 {
        self.sdo_read_u32(OD_MOTOR_RATED_TORQUE, 0)
    }

    pub fn set_motor_rated_torque(&self, value: u32) {
        self.sdo_write_u32(OD_MOTOR_RATED_TORQUE, 0, value);
    }

    pub fn position_offset(&self) -> i32 {
        self.sdo_read_i32(OD_POSITION_OFFSET, 0)
    }

    pub fn set_position_offset(&self, value: i32) {
        self.sdo_write_i32(OD_POSITION_OFFSET, 0, value);
    }

    pub fn velocity_offset(&self) -> i32 {
        self.sdo_read_i32(OD_VELOCITY_OFFSET, 0)
    }

    pub fn set_velocity_offset(&self, value: i32) {
        self.sdo_write_i32(OD_VELOCITY_OFFSET, 0, value);
    }

    pub fn torque_offset(&self) -> i16 {
        self.sdo_read_i16(OD_TORQUE_OFFSET, 0)
    }

    pub fn set_torque_offset(&self, value: i16) {
        self.sdo_write_i16(OD_TORQUE_OFFSET, 0, value);
    }

    pub fn interpolation_time_period_value(&self) -> u8 {
        self.sdo_read_u8(OD_INTERPOLATION_TIME_PERIOD, 1)
    }

    pub fn set_interpolation_time_period_value(&self, value: u8) {
        self.sdo_write_u8(OD_INTERPOLATION_TIME_PERIOD, 1, value);
    }

    pub fn interpolation_time_period_index(&self) -> i8 {
        self.sdo_read_i8(OD_INTERPOLATION_TIME_PERIOD, 2)
    }

    pub fn set_interpolation_time_period_index(&self, value: i8) {
        self.sdo_write_i8(OD_INTERPOLATION_TIME_PERIOD, 2, value);
    }

    pub fn quick_stop_option_code(&self) -> i16 {
        self.sdo_read_i16(OD_QUICK_STOP_OPTION_CODE, 0)
    }

    pub fn set_quick_stop_option_code(&self, value: i16) {
        self.sdo_write_i16(OD_QUICK_STOP_OPTION_CODE, 0, value);
    }

    pub fn positive_torque_limit(&self) -> u16 {
        self.sdo_read_u16(OD_POSITIVE_TORQUE_LIMIT, 0)
    }

    pub fn set_positive_torque_limit(&self, value: u16) {
        self.sdo_write_u16(OD_POSITIVE_TORQUE_LIMIT, 0, value);
    }

    pub fn negative_torque_limit(&self) -> u16 {
        self.sdo_read_u16(OD_NEGATIVE_TORQUE_LIMIT, 0)
    }

    pub fn set_negative_torque_limit(&self, value: u16) {
        self.sdo_write_u16(OD_NEGATIVE_TORQUE_LIMIT, 0, value);
    }

    pub fn supported_homing_methods(&self) -> Vec<i8> {
        let count = self.sdo_read_u8(OD_SUPPORTED_HOMING_METHODS, 0);
        let mut methods = Vec::new();
        for i in 1..=count {
            methods.push(self.sdo_read_i8(OD_SUPPORTED_HOMING_METHODS, i));
        }
        methods
    }

    pub fn txpdo_data_invalid(&self) -> bool {
        self.sdo_read_u8(OD_TXPDO_DATA_INVALID, 0) != 0
    }

    pub fn synchronization_settings(&self) -> u16 {
        self.sdo_read_u16(OD_SYNCHRONIZATION_SETTINGS, 1)
    }

    pub fn set_synchronization_settings(&self, value: u16) {
        self.sdo_write_u16(OD_SYNCHRONIZATION_SETTINGS, 1, value);
    }

    pub fn drive_sync_status(&self) -> u16 {
        self.sdo_read_u16(OD_DRIVE_SYNC_STATUS, 0)
    }

    pub fn new_setpoint(&self, position: i32, relative: bool) {
        self.set_target_position(position);
        let mut cw = CW_ENABLE_OPERATION | 0x10;
        if relative { cw |= 0x40; }
        self.set_controlword(cw);
    }

    pub fn clear_new_setpoint(&self) {
        self.set_controlword(CW_ENABLE_OPERATION);
    }

    pub fn start_homing(&self) {
        self.set_controlword(CW_ENABLE_OPERATION | 0x10);
    }

    pub fn homing_attained(&self) -> bool {
        (self.statusword() & SW_OP_MODE_SPECIFIC_1) != 0
    }

    pub fn homing_error(&self) -> bool {
        (self.statusword() & SW_OP_MODE_SPECIFIC_2) != 0
    }

    pub fn validate_profile_parameters(&self, configured_mode: Option<i8>) -> Vec<String> {
        let mut warnings = Vec::new();

        let mode = configured_mode.unwrap_or_else(|| self.sdo_read_i8(OD_MODES_OF_OPERATION, 0));

        if mode != 1 && mode != 3 {
            return warnings;
        }

        let mode_name = if mode == 1 { "PP" } else { "PV" };
        let checks: [(u16, &str); 3] = [
            (OD_MAX_PROFILE_VELOCITY, "Max Profile Velocity (0x6080)"),
            (OD_PROFILE_ACCELERATION, "Profile Acceleration (0x6083)"),
            (OD_PROFILE_DECELERATION, "Profile Deceleration (0x6084)"),
        ];

        for (index, name) in &checks {
            let val = self.sdo_read_u32(*index, 0);
            if val == 0 {
                warnings.push(format!(
                    "{} 模式: {} = 0, 驱动器无法生成运动曲线, 请在 Startup 中配置此参数",
                    mode_name, name
                ));
            }
        }

        warnings
    }

    fn try_read_attribute<T: Cia402Readable>(&self, index: u16, subindex: u8)
        -> (T, Cia402ReadStatus, u32)
    {
        let mut size: c_int = 0;
        let ptr = unsafe {
            ffi::SDOread(self.master_index, self.slave_index, index, subindex, 0, &mut size)
        };
        if !ptr.is_null() && size > 0 {

            let data = unsafe { std::slice::from_raw_parts(ptr, size as usize).to_vec() };
            unsafe { ffi::FreeMemory(ptr as *mut _) };
            return (T::from_le_slice(&data), Cia402ReadStatus::Success, 0);
        }

        if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
        let abort_code = if size < 0 { (-size) as u32 } else { 0 };
        let status = if abort_code != 0 {
            Cia402ReadStatus::classify_abort(abort_code)
        } else {
            Cia402ReadStatus::ReadFailed
        };
        (T::default(), status, abort_code)
    }

    pub fn try_read_object<T: Cia402Readable>(&self, index: u16, subindex: u8)
        -> Cia402ReadResult<T>
    {
        let (value, status, abort) = self.try_read_attribute::<T>(index, subindex);
        Cia402ReadResult::new(value, status, abort)
    }

    pub fn try_read_error_code(&self) -> Cia402ReadResult<u16> {
        self.try_read_object::<u16>(OD_ERROR_CODE, 0)
    }

    pub fn try_read_supported_drive_modes(&self) -> Cia402ReadResult<u32> {
        self.try_read_object::<u32>(OD_SUPPORTED_DRIVE_MODES, 0)
    }

    pub fn try_read_digital_inputs(&self) -> Cia402ReadResult<u32> {
        self.try_read_object::<u32>(OD_DIGITAL_INPUTS, 0)
    }

    pub fn try_read_operation_mode_display(&self) -> Cia402ReadResult<i8> {
        self.try_read_object::<i8>(OD_MODES_OF_OPERATION_DISPLAY, 0)
    }

    pub fn try_read_drive_data(&self, subindex: u8) -> Cia402ReadResult<u32> {
        self.try_read_object::<u32>(OD_DRIVE_DATA, subindex)
    }

    pub fn try_read_statusword(&self) -> Cia402ReadResult<u16> {
        self.try_read_object::<u16>(OD_STATUSWORD, 0)
    }

    pub fn try_read_position_actual(&self) -> Cia402ReadResult<i32> {
        self.try_read_object::<i32>(OD_POSITION_ACTUAL, 0)
    }

    pub fn try_read_torque_actual(&self) -> Cia402ReadResult<i16> {
        self.try_read_object::<i16>(OD_TORQUE_ACTUAL, 0)
    }

    pub fn supported_drive_modes_readable(&self) -> bool {
        self.try_read_supported_drive_modes().ok()
    }

    pub fn transition_command(current: StateCiA402, target: StateCiA402) -> u16 {
        unsafe { ffi::CiA402_GetTransitionCommand(current as c_int, target as c_int) }
    }

    pub fn request_transition(&self, target: StateCiA402) -> bool {
        unsafe {
            ffi::CiA402_RequestTransition(self.master_index, self.slave_index, target as c_int) != 0
        }
    }

    pub fn fault_reset_step(&self, step: i32) -> bool {
        unsafe {
            ffi::CiA402_FaultResetStep(self.master_index, self.slave_index, step) != 0
        }
    }

    pub fn supported_drive_modes(&self) -> u32 {
        unsafe { ffi::CiA402_GetSupportedDriveModes(self.master_index, self.slave_index) }
    }

    pub fn is_mode_supported(&self, mode: ModeCiA402) -> bool {
        unsafe {
            ffi::CiA402_IsModeSupported(self.master_index, self.slave_index, mode as c_int) != 0
        }
    }

    pub fn drive_error_code(&self) -> u16 {
        unsafe { ffi::CiA402_GetErrorCode(self.master_index, self.slave_index) }
    }

    pub fn error_history(&self) -> Vec<u32> {
        const MAX: usize = 64;
        let mut buf = [0u32; MAX];
        let n = unsafe {
            ffi::CiA402_GetErrorHistory(
                self.master_index, self.slave_index, buf.as_mut_ptr(), MAX as c_int,
            )
        };
        if n <= 0 { return Vec::new(); }
        buf[..(n as usize).min(MAX)].to_vec()
    }

    pub fn clear_error_history(&self) -> bool {
        unsafe { ffi::CiA402_ClearErrorHistory(self.master_index, self.slave_index) != 0 }
    }

    pub fn set_homing_method(&self, method: i8) -> bool {
        unsafe { ffi::CiA402_SetHomingMethod(self.master_index, self.slave_index, method) != 0 }
    }

    pub fn homing_method(&self) -> i8 {
        unsafe { ffi::CiA402_GetHomingMethod(self.master_index, self.slave_index) }
    }

    pub fn supported_homing_methods_native(&self) -> Vec<i8> {
        const MAX: usize = 64;
        let mut buf = [0i8; MAX];
        let n = unsafe {
            ffi::CiA402_GetSupportedHomingMethods(
                self.master_index, self.slave_index, buf.as_mut_ptr(), MAX as c_int,
            )
        };
        if n <= 0 { return Vec::new(); }
        buf[..(n as usize).min(MAX)].to_vec()
    }

    pub fn is_standard_homing_method(method: i8) -> bool {
        unsafe { ffi::CiA402_IsStandardHomingMethod(method) != 0 }
    }

    pub fn quick_stop_option_native(&self) -> i16 {
        unsafe { ffi::CiA402_GetQuickStopOption(self.master_index, self.slave_index) }
    }

    pub fn set_quick_stop_option_native(&self, value: i16) -> bool {
        unsafe { ffi::CiA402_SetQuickStopOption(self.master_index, self.slave_index, value) != 0 }
    }

    pub fn shutdown_option(&self) -> i16 {
        unsafe { ffi::CiA402_GetShutdownOption(self.master_index, self.slave_index) }
    }

    pub fn set_shutdown_option(&self, value: i16) -> bool {
        unsafe { ffi::CiA402_SetShutdownOption(self.master_index, self.slave_index, value) != 0 }
    }

    pub fn disable_operation_option(&self) -> i16 {
        unsafe { ffi::CiA402_GetDisableOperationOption(self.master_index, self.slave_index) }
    }

    pub fn set_disable_operation_option(&self, value: i16) -> bool {
        unsafe {
            ffi::CiA402_SetDisableOperationOption(self.master_index, self.slave_index, value) != 0
        }
    }

    pub fn halt_option(&self) -> i16 {
        unsafe { ffi::CiA402_GetHaltOption(self.master_index, self.slave_index) }
    }

    pub fn set_halt_option(&self, value: i16) -> bool {
        unsafe { ffi::CiA402_SetHaltOption(self.master_index, self.slave_index, value) != 0 }
    }

    pub fn fault_reaction_option(&self) -> i16 {
        unsafe { ffi::CiA402_GetFaultReactionOption(self.master_index, self.slave_index) }
    }

    pub fn set_fault_reaction_option(&self, value: i16) -> bool {
        unsafe {
            ffi::CiA402_SetFaultReactionOption(self.master_index, self.slave_index, value) != 0
        }
    }
}

impl std::fmt::Display for CiA402Instance {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        let sw = self.statusword();
        let state = StateCiA402::from_statusword(sw);
        let mode = self.operation_mode();
        write!(f, "从站 {}: CiA402 [{}, 模式={}, SW=0x{:04X}]",
            self.slave_index, state, crate::statics::print::cia402_mode_description(mode), sw)
    }
}
