
use std::collections::HashMap;
use std::sync::{Arc, Mutex};
use std::time::SystemTime;
use crate::data::error::{DarraError, Result};
use crate::utils::ffi::{self, FsoeConfig, SafeMdpConfig, FsoeStatus, FsoeState, FsoeError};

pub struct FsoeCrc16;

impl FsoeCrc16 {

    const TABLE: [u16; 256] = Self::build_table();

    const fn build_table() -> [u16; 256] {
        let mut table = [0u16; 256];
        let mut i = 0usize;
        while i < 256 {
            let mut crc = (i as u16) << 8;
            let mut j = 0;
            while j < 8 {
                if (crc & 0x8000) != 0 {
                    crc = (crc << 1) ^ 0x1021;
                } else {
                    crc <<= 1;
                }
                j += 1;
            }
            table[i] = crc;
            i += 1;
        }
        table
    }

    pub fn compute(data: &[u8]) -> u16 {
        let mut crc: u16 = 0xFFFF;
        for &byte in data {
            let idx = (((crc >> 8) as u8) ^ byte) as usize;
            crc = (crc << 8) ^ Self::TABLE[idx];
        }
        crc
    }

    pub fn update(crc: u16, data: &[u8]) -> u16 {
        let mut crc = crc;
        for &byte in data {
            let idx = (((crc >> 8) as u8) ^ byte) as usize;
            crc = (crc << 8) ^ Self::TABLE[idx];
        }
        crc
    }

    pub fn verify(data: &[u8]) -> bool {
        if data.len() < 2 {
            return false;
        }
        let payload = &data[..data.len() - 2];
        let expected = u16::from_le_bytes([data[data.len() - 2], data[data.len() - 1]]);
        Self::compute(payload) == expected
    }

    pub fn append(data: &[u8]) -> Vec<u8> {
        let crc = Self::compute(data);
        let mut out = data.to_vec();
        out.extend_from_slice(&crc.to_le_bytes());
        out
    }
}

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

    pub slave_index: u16,

    pub config: FsoeConfig,

    pub last_status: Option<FsoeStatus>,
}

pub struct SafetyManager {

    master_index: u16,

    connections: HashMap<u16, FsoeConnectionInfo>,
}

impl SafetyManager {

    pub fn new(master_index: u16) -> Self {
        Self {
            master_index,
            connections: HashMap::new(),
        }
    }

    pub fn add_connection(&mut self, slave_index: u16, config: FsoeConfig) -> Result<()> {
        let mut cfg = config;
        let ret = unsafe {
            ffi::FSoEInitConnection(self.master_index, slave_index, &mut cfg)
        };

        if ret == 0 {
            return Err(DarraError::Other(format!(
                "FSoE 连接初始化失败 (从站: {})", slave_index
            )));
        }
        self.connections.insert(slave_index, FsoeConnectionInfo {
            slave_index,
            config: cfg,
            last_status: None,
        });
        Ok(())
    }

    pub fn remove_connection(&mut self, slave_index: u16) -> Result<()> {
        let ret = unsafe {
            ffi::FSoECloseConnection(self.master_index, slave_index)
        };
        self.connections.remove(&slave_index);

        if ret == 0 {
            Err(DarraError::Other(format!(
                "FSoE 连接关闭失败 (从站: {})", slave_index
            )))
        } else {
            Ok(())
        }
    }

    pub fn refresh_all_status(&mut self) -> HashMap<u16, FsoeStatus> {
        let mut result = HashMap::new();
        let keys: Vec<u16> = self.connections.keys().copied().collect();
        for slave in keys {
            let mut status = FsoeStatus {
                state: FsoeState::Reset,
                last_error: FsoeError::None,
                error_count: 0,
                frames_sent: 0,
                frames_received: 0,
                crc_errors: 0,
                watchdog_errors: 0,
                watchdog_expired: 0,
                in_failsafe: 0,
                toggle_bit: 0,
                initialized: 0,
            };
            let ret = unsafe {
                ffi::FSoEGetStatus(self.master_index, slave, &mut status)
            };
            if ret != 0 {
                if let Some(info) = self.connections.get_mut(&slave) {
                    info.last_status = Some(status);
                }
                result.insert(slave, status);
            }
        }
        result
    }

    pub fn all_in_data(&self) -> HashMap<u16, Vec<u8>> {
        let mut result = HashMap::new();
        for (&slave, info) in &self.connections {
            let size = info.config.safe_input_size as usize;
            if size == 0 { continue; }
            let mut buf = vec![0u8; size];
            let mut actual_size = size as u32;
            let ret = unsafe {
                ffi::FSoEReadSafeInput(self.master_index, slave, buf.as_mut_ptr(), &mut actual_size)
            };
            if ret != 0 {
                buf.truncate(actual_size as usize);
                result.insert(slave, buf);
            }
        }
        result
    }

    pub fn any_in_failsafe(&mut self) -> bool {
        let statuses = self.refresh_all_status();
        statuses.values().any(|s| s.in_failsafe != 0)
    }

    pub fn enter_all_failsafe(&self) -> Vec<(u16, Result<()>)> {
        let mut results = Vec::new();
        for (&slave, info) in &self.connections {
            let size = info.config.safe_output_size as usize;
            let zeros = vec![0u8; size];
            let ret = unsafe {
                ffi::FSoESetFailsafeOutput(self.master_index, slave, zeros.as_ptr(), size as u32)
            };
            let result = if ret != 0 {
                Ok(())
            } else {
                Err(DarraError::Other(format!(
                    "从站 {} 进入失效安全失败 (返回码: {})", slave, ret
                )))
            };
            results.push((slave, result));
        }
        results
    }

    pub fn process_cycle(&self) {
        unsafe { ffi::FSoEProcessCycle(self.master_index) };
    }

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

    pub fn dll_connection_count(&self) -> u16 {
        unsafe { ffi::FSoEGetConnectionCount(self.master_index) }
    }

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

    pub fn state(&self, slave_index: u16) -> Option<FsoeState> {
        let raw = unsafe { ffi::FSoEGetState(self.master_index, slave_index) };

        match raw {
            0x100 => Some(FsoeState::Reset),
            0x101 => Some(FsoeState::Session),
            0x102 => Some(FsoeState::Connection),
            0x103 => Some(FsoeState::Parameter),
            0x104 => Some(FsoeState::Data),
            0x105 => Some(FsoeState::Failsafe),
            _ => None,
        }
    }

    pub fn write_safe_output(&self, slave_index: u16, data: &[u8]) -> Result<()> {
        let ret = unsafe {
            ffi::FSoEWriteSafeOutput(self.master_index, slave_index, data.as_ptr(), data.len() as u32)
        };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!("FSoE 安全输出写入失败 (从站: {})", slave_index)))
        }
    }

    pub fn download_parameters(&self, slave_index: u16, params: &[u8]) -> Result<u32> {
        let mut sra_crc: u32 = 0;
        let ret = unsafe {
            ffi::FSoEDownloadParameters(
                self.master_index, slave_index,
                params.as_ptr(), params.len() as u32, &mut sra_crc,
            )
        };
        if ret != 0 { Ok(sra_crc) } else {
            Err(DarraError::Other(format!("FSoE 参数下载失败 (从站: {})", slave_index)))
        }
    }

    pub fn reset_connection(&self, slave_index: u16) -> Result<()> {
        let ret = unsafe { ffi::FSoEReset(self.master_index, slave_index) };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!("FSoE 连接重置失败 (从站: {})", slave_index)))
        }
    }

    pub fn clear_error(&self, slave_index: u16) {
        unsafe { ffi::FSoEClearError(self.master_index, slave_index) };
    }

    pub fn find_by_address(&self, safety_address: u16) -> Option<u16> {
        for (&slave, info) in &self.connections {
            if info.config.safety_address == safety_address {
                return Some(slave);
            }
        }
        None
    }

    pub fn find_by_connection_id(&self, connection_id: u16) -> Option<u16> {
        for (&slave, info) in &self.connections {
            if info.config.connection_id == connection_id {
                return Some(slave);
            }
        }
        None
    }

    pub fn find_connection_by_address(&self, safety_address: u16) -> Option<u16> {
        self.find_by_address(safety_address)
    }

    pub fn bind_safe_input(&mut self, slave_index: u16, safety_address: u16,
                            input_size: u16, watchdog_ms: u32) -> Result<()> {
        let config = FsoeConfig {
            connection_id: slave_index + 1,
            safety_address,
            watchdog_time_ms: watchdog_ms,
            safe_input_size: input_size,
            safe_output_size: 0,
            pdo_input_offset: 0,
            pdo_output_offset: 0,
        };
        self.add_connection(slave_index, config)
    }

    pub fn bind_safe_output(&mut self, slave_index: u16, safety_address: u16,
                             output_size: u16, watchdog_ms: u32) -> Result<()> {
        let config = FsoeConfig {
            connection_id: slave_index + 1,
            safety_address,
            watchdog_time_ms: watchdog_ms,
            safe_input_size: 0,
            safe_output_size: output_size,
            pdo_input_offset: 0,
            pdo_output_offset: 0,
        };
        self.add_connection(slave_index, config)
    }

    pub fn bind_safe_io(&mut self, slave_index: u16,
                        safe_input_offset: u32, safe_input_size: u16,
                        safe_output_offset: u32, safe_output_size: u16,
                        safety_address: u16, watchdog_ms: u32) -> Result<()> {
        let config = FsoeConfig {
            connection_id: slave_index + 1,
            safety_address,
            watchdog_time_ms: watchdog_ms,
            safe_input_size,
            safe_output_size,
            pdo_input_offset: safe_input_offset,
            pdo_output_offset: safe_output_offset,
        };
        self.add_connection(slave_index, config)
    }

    pub fn bind_mdp_drive_axis(&mut self, slave_index: u16, axis_index: u16,
                               safety_address: u16, watchdog_ms: u32) -> Result<()> {

        let conn_index = axis_index + 1;
        let mdp = SafeMdp::new(self.master_index, slave_index);

        let config = SafeMdpConfig {
            connection_id: conn_index,
            safety_address,
            watchdog_time_ms: watchdog_ms,
            safe_input_size: 0,
            safe_output_size: 0,
            pdo_input_offset: 0,
            pdo_output_offset: 0,
            module_number: axis_index,
            module_profile: 790,
            axis_number: axis_index,
            connection_type: 0,
        };
        mdp.init_connection(conn_index, config)
    }

    pub fn write_output_frame(&self, slave_index: u16, frame_data: &[u8]) -> bool {
        if frame_data.is_empty() {
            return false;
        }
        if !self.connections.contains_key(&slave_index) {
            return false;
        }
        let ret = unsafe {
            ffi::FSoEWriteSafeOutput(
                self.master_index, slave_index,
                frame_data.as_ptr(), frame_data.len() as u32,
            )
        };
        ret != 0
    }

    pub fn read_input_frame(&self, slave_index: u16, buffer_size: usize) -> Option<Vec<u8>> {
        let info = self.connections.get(&slave_index)?;
        let read_size = if buffer_size > 0 {
            buffer_size
        } else {
            info.config.safe_input_size as usize
        };
        if read_size == 0 {
            return None;
        }
        let mut buf = vec![0u8; read_size];
        let mut actual_size = buf.len() as u32;
        let ret = unsafe {
            ffi::FSoEReadSafeInput(
                self.master_index, slave_index,
                buf.as_mut_ptr(), &mut actual_size,
            )
        };
        if ret != 0 && actual_size > 0 {
            buf.truncate(actual_size as usize);
            Some(buf)
        } else {
            None
        }
    }

    pub fn status_summary(&self) -> String {
        if self.connections.is_empty() {
            return "未初始化".to_string();
        }
        let mut lines = vec![format!("FSoE Manager: 连接数={}", self.connections.len())];
        for (&slave, info) in &self.connections {
            let state = self.state(slave)
                .map(|s| format!("{:?}", s))
                .unwrap_or_else(|| "未知".to_string());
            lines.push(format!("  连接[从站={}]: 状态={}, 地址=0x{:04X}",
                slave, state, info.config.safety_address));
        }
        lines.join("\n")
    }
}

impl Drop for SafetyManager {

    fn drop(&mut self) {
        let keys: Vec<u16> = self.connections.keys().copied().collect();
        for slave in keys {
            unsafe { ffi::FSoECloseConnection(self.master_index, slave) };
        }
    }
}

pub struct SafeMdp {

    master_index: u16,

    slave_index: u16,
}

impl SafeMdp {

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

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

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

    pub fn device_address(&self) -> Result<u16> {
        let mut safety_addr: u16 = 0;
        let ret = unsafe {
            ffi::SafeMdpGetDeviceAddress(self.master_index, self.slave_index, &mut safety_addr)
        };
        if ret != 0 { Ok(safety_addr) } else {
            Err(DarraError::Other(format!(
                "读取 MDP 设备地址失败 (从站: {})", self.slave_index
            )))
        }
    }

    pub fn init_connection(&self, connection_index: u16, config: SafeMdpConfig) -> Result<()> {
        let mut cfg = config;
        let ret = unsafe {
            ffi::SafeMdpInitConnection(
                self.master_index, self.slave_index, connection_index, &mut cfg,
            )
        };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 初始化失败 (从站: {})", connection_index, self.slave_index
            )))
        }
    }

    pub fn close_connection(&self, connection_index: u16) -> Result<()> {
        let ret = unsafe {
            ffi::SafeMdpCloseConnection(self.master_index, self.slave_index, connection_index)
        };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 关闭失败 (从站: {})", connection_index, self.slave_index
            )))
        }
    }

    pub fn status(&self, connection_index: u16) -> Result<FsoeStatus> {
        let mut status = FsoeStatus {
            state: FsoeState::Reset,
            last_error: FsoeError::None,
            error_count: 0,
            frames_sent: 0,
            frames_received: 0,
            crc_errors: 0,
            watchdog_errors: 0,
            watchdog_expired: 0,
            in_failsafe: 0,
            toggle_bit: 0,
            initialized: 0,
        };
        let ret = unsafe {
            ffi::SafeMdpGetStatus(
                self.master_index, self.slave_index, connection_index, &mut status,
            )
        };
        if ret != 0 { Ok(status) } else {
            Err(DarraError::Other(format!(
                "获取 FSoE MDP 连接 {} 状态失败 (从站: {})", connection_index, self.slave_index
            )))
        }
    }

    pub fn request_state(&self, connection_index: u16, target_state: FsoeState) -> Result<()> {
        let ret = unsafe {
            ffi::SafeMdpRequestState(
                self.master_index, self.slave_index, connection_index, target_state as i32,
            )
        };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 状态请求失败", connection_index
            )))
        }
    }

    pub fn state(&self, connection_index: u16) -> Option<FsoeState> {
        let raw = unsafe {
            ffi::SafeMdpGetState(self.master_index, self.slave_index, connection_index)
        };
        match raw {
            0x100 => Some(FsoeState::Reset),
            0x101 => Some(FsoeState::Session),
            0x102 => Some(FsoeState::Connection),
            0x103 => Some(FsoeState::Parameter),
            0x104 => Some(FsoeState::Data),
            0x105 => Some(FsoeState::Failsafe),
            _ => None,
        }
    }

    pub fn reset(&self, connection_index: u16) -> Result<()> {
        let ret = unsafe {
            ffi::SafeMdpReset(self.master_index, self.slave_index, connection_index)
        };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 重置失败", connection_index
            )))
        }
    }

    pub fn write_safe_output(&self, connection_index: u16, data: &[u8]) -> Result<()> {
        let ret = unsafe {
            ffi::SafeMdpWriteSafeOutput(
                self.master_index, self.slave_index, connection_index,
                data.as_ptr(), data.len() as u32,
            )
        };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 安全输出写入失败", connection_index
            )))
        }
    }

    pub fn read_safe_input(&self, connection_index: u16, max_size: usize) -> Result<Vec<u8>> {
        let mut buf = vec![0u8; max_size];
        let mut actual_size = max_size as u32;
        let ret = unsafe {
            ffi::SafeMdpReadSafeInput(
                self.master_index, self.slave_index, connection_index,
                buf.as_mut_ptr(), &mut actual_size,
            )
        };
        if ret != 0 {
            buf.truncate(actual_size as usize);
            Ok(buf)
        } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 安全输入读取失败", connection_index
            )))
        }
    }

    pub fn download_parameters(&self, connection_index: u16, params: &[u8]) -> Result<u32> {
        let mut sra_crc: u32 = 0;
        let ret = unsafe {
            ffi::SafeMdpDownloadParameters(
                self.master_index, self.slave_index, connection_index,
                params.as_ptr(), params.len() as u32, &mut sra_crc,
            )
        };
        if ret != 0 { Ok(sra_crc) } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 参数下载失败", connection_index
            )))
        }
    }

    pub fn set_failsafe_output(&self, connection_index: u16, data: &[u8]) -> Result<()> {
        let ret = unsafe {
            ffi::SafeMdpSetFailsafeOutput(
                self.master_index, self.slave_index, connection_index,
                data.as_ptr(), data.len() as u32,
            )
        };
        if ret != 0 { Ok(()) } else {
            Err(DarraError::Other(format!(
                "FSoE MDP 连接 {} 失效安全输出设置失败", connection_index
            )))
        }
    }

    pub fn check_watchdog(&self, connection_index: u16) -> bool {
        unsafe {
            ffi::SafeMdpCheckWatchdog(self.master_index, self.slave_index, connection_index) != 0
        }
    }

    pub fn last_error(&self, connection_index: u16) -> i32 {
        unsafe {
            ffi::SafeMdpGetLastError(self.master_index, self.slave_index, connection_index)
        }
    }

    pub fn clear_error(&self, connection_index: u16) {
        unsafe {
            ffi::SafeMdpClearError(self.master_index, self.slave_index, connection_index)
        };
    }

    pub fn module_comm_param(&self, module_number: u16) -> Result<Vec<u8>> {
        let mut buf = vec![0u8; 256];
        let mut size: u32 = 256;
        let ret = unsafe {
            ffi::SafeMdpGetModuleCommParam(
                self.master_index, self.slave_index, module_number,
                buf.as_mut_ptr(), &mut size,
            )
        };
        if ret != 0 {
            buf.truncate(size as usize);
            Ok(buf)
        } else {
            Err(DarraError::Other(format!(
                "读取 MDP 模块 {} 通信参数失败", module_number
            )))
        }
    }

    pub fn module_diagnosis(&self, module_number: u16) -> Result<(u16, u16)> {
        let mut conn_state: u16 = 0;
        let mut conn_diag: u16 = 0;
        let ret = unsafe {
            ffi::SafeMdpGetModuleDiagnosis(
                self.master_index, self.slave_index, module_number,
                &mut conn_state, &mut conn_diag,
            )
        };
        if ret != 0 { Ok((conn_state, conn_diag)) } else {
            Err(DarraError::Other(format!(
                "读取 MDP 模块 {} 诊断数据失败", module_number
            )))
        }
    }
}

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

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

    Reset = 0x2A,

    Session = 0x4E,

    Connection = 0x64,

    Parameter = 0x52,

    Failsafe = 0x08,

    Data = 0x36,
}

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum FsoeFailsafeReason {

    WatchdogTimeout,

    CrcError,

    CommunicationError,

    ApplicationRequest,

    SlaveRequest,

    MasterRequest,

    RecoveryToData,
}

#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum FsoeConnectionMode {

    Single,

    Multiple,
}

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

    Master = 0,

    Slave = 1,
}

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

    DigitalInput = 190,

    DigitalInOut = 195,

    DigitalOutput = 290,

    DriveConnection = 790,

    Master = 6900,
}

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

    pub connection_id: u16,

    pub safety_address: u16,

    pub watchdog_time_ms: u32,

    pub safe_input_size: u16,

    pub safe_output_size: u16,

    pub pdo_input_offset: u32,

    pub pdo_output_offset: u32,
}

impl Default for FsoeConnectionConfig {
    fn default() -> Self {
        Self {
            connection_id: 0,
            safety_address: 0,
            watchdog_time_ms: 100,
            safe_input_size: 0,
            safe_output_size: 0,
            pdo_input_offset: 0,
            pdo_output_offset: 0,
        }
    }
}

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

    pub state: FsoeState,

    pub last_error: FsoeError,

    pub error_count: u32,

    pub frames_sent: u32,

    pub frames_received: u32,

    pub crc_errors: u32,

    pub watchdog_errors: u32,

    pub watchdog_expired: bool,

    pub in_failsafe: bool,

    pub toggle_bit: u8,

    pub is_initialized: bool,
}

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

    pub connection_id: u16,

    pub safety_address: u16,

    pub watchdog_time_ms: u32,

    pub comm_param_length: u16,

    pub app_param_length: u16,

    pub version: String,

    pub connection_type: u16,
}

impl Default for FsoeCommParameters {
    fn default() -> Self {
        Self {
            connection_id: 0,
            safety_address: 0,
            watchdog_time_ms: 0,
            comm_param_length: 0,
            app_param_length: 0,
            version: "01".to_string(),
            connection_type: 0,
        }
    }
}

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

    NoError = 0x0000,

    WrongCommand = 0x0001,

    UnknownCommand = 0x0002,

    WrongConnectionId = 0x0003,

    WrongCrc = 0x0004,

    WatchdogExpired = 0x0005,

    WrongFsoeAddress = 0x0006,

    WrongData = 0x0007,

    WrongCommParamLength = 0x0008,

    WrongCommParam = 0x0009,

    WrongApplParamLength = 0x000A,

    WrongApplParam = 0x000B,

    UnexpectedSessionCmd = 0x000C,

    FailsafeDataReceived = 0x000D,

    ErrorMask = 0x000F,

    ErrorInSlave = 0x0010,

    SlaveInFailsafe = 0x0020,

    ConnectionInStartup = 0x0040,

    MasterInFailsafe = 0x0080,
}

pub struct FsoeConstants;

impl FsoeConstants {

    pub const MAX_CONNECTIONS: u32 = 64;

    pub const MAX_SAFE_DATA_SIZE: u32 = 64;

    pub const MAX_CRC_SEGMENTS: u32 = 32;

    pub const FRAME_HEADER_SIZE: u32 = 3;

    pub const FRAME_CRC_SIZE: u32 = 2;

    pub const CRC16_POLY: u16 = 0x1021;

    pub const CRC16_INITIAL: u16 = 0xFFFF;

    pub const SRA_CRC32_POLY: u32 = 0x814141AB;

    pub const DEFAULT_WATCHDOG_TIME_MS: u32 = 100;

    pub const COMMAND_TYPE_MASK: u8 = 0x0F;

    pub const TOGGLE_BIT_MASK: u8 = 0x80;

    pub const MASTER_RESET_MASK: u8 = 0x40;

    pub const SLAVE_RESET_MASK: u8 = 0x20;
}

pub struct FsoeHelper;

impl FsoeHelper {

    pub fn get_command_type(command: u8) -> Option<FsoeCommand> {
        match command {
            0x2A => Some(FsoeCommand::Reset),
            0x4E => Some(FsoeCommand::Session),
            0x64 => Some(FsoeCommand::Connection),
            0x52 => Some(FsoeCommand::Parameter),
            0x08 => Some(FsoeCommand::Failsafe),
            0x36 => Some(FsoeCommand::Data),
            _ => None,
        }
    }

    pub fn get_toggle_bit(command: u8) -> bool {
        let _ = command;
        false
    }

    pub fn is_master_reset_request(command: u8) -> bool {
        command == FsoeCommand::Reset as u8
    }

    pub fn is_slave_reset_request(command: u8) -> bool {
        command == FsoeCommand::Reset as u8
    }

    pub fn build_command(
        cmd_type: FsoeCommand,
        toggle_bit: bool,
        master_reset: bool,
        slave_reset: bool,
    ) -> u8 {
        let _ = (toggle_bit, master_reset, slave_reset);
        cmd_type as u8
    }

    pub fn is_valid_state_transition(current: FsoeState, target: FsoeState) -> bool {
        match current {
            FsoeState::Reset => matches!(target, FsoeState::Session),
            FsoeState::Session => matches!(target, FsoeState::Connection | FsoeState::Reset),
            FsoeState::Connection => matches!(target, FsoeState::Parameter | FsoeState::Reset),
            FsoeState::Parameter => matches!(target, FsoeState::Data | FsoeState::Reset),
            FsoeState::Data => matches!(target, FsoeState::Failsafe | FsoeState::Reset),
            FsoeState::Failsafe => matches!(target, FsoeState::Data | FsoeState::Reset),
        }
    }

    pub fn get_error_description(error: FsoeError) -> &'static str {
        match error {
            FsoeError::None => "无错误",
            FsoeError::WrongCommand => "错误的命令",
            FsoeError::UnknownCommand => "未知命令",
            FsoeError::WrongConnId => "连接ID不匹配",
            FsoeError::Crc => "CRC校验失败",
            FsoeError::Watchdog => "看门狗超时",
            FsoeError::WrongAddress => "错误的FSoE地址",
            FsoeError::WrongData => "无效数据",
            FsoeError::CommParamLength => "通信参数长度错误",
            FsoeError::CommParam => "通信参数错误",
            FsoeError::AppParamLength => "应用参数长度错误",
            FsoeError::AppParam => "应用参数错误",
            FsoeError::UnexpectedSession => "意外的会话命令",
            FsoeError::FailsafeData => "收到失效安全数据",
            FsoeError::NotInitialized => "FSoE未初始化",
            FsoeError::MaxConnections => "达到最大连接数",
            FsoeError::InvalidState => "无效状态转换",
        }
    }
}

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

    pub slave_index: u16,

    pub connection_index: u16,

    pub old_state: FsoeState,

    pub new_state: FsoeState,
}

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

    pub slave_index: u16,

    pub connection_index: u16,

    pub error: FsoeError,

    pub current_state: FsoeState,
}

impl FsoeErrorEventArgs {

    pub fn description(&self) -> &'static str {
        FsoeHelper::get_error_description(self.error)
    }
}

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

    pub slave_index: u16,

    pub connection_index: u16,

    pub reason: FsoeFailsafeReason,

    pub entering_failsafe: bool,
}

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

    pub slave_index: u16,

    pub connection_index: u16,

    pub is_input: bool,

    pub data: Vec<u8>,
}

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

    pub slave_index: u16,

    pub current_state: FsoeState,

    pub safe_inputs: Vec<u8>,

    pub safe_outputs: Vec<u8>,

    pub in_failsafe: bool,

    pub cycle_count: u32,

    pub timestamp: SystemTime,
}

pub type FsoeDataExchangeCallback =
    Arc<dyn Fn(&FsoeDataExchangeEventArgs) + Send + Sync>;

pub struct FsoeDataExchangeDispatcher {
    callbacks: Arc<Mutex<Vec<FsoeDataExchangeCallback>>>,
    cycle_count: Arc<Mutex<u32>>,
}

impl FsoeDataExchangeDispatcher {

    pub fn new() -> Self {
        Self {
            callbacks: Arc::new(Mutex::new(Vec::new())),
            cycle_count: Arc::new(Mutex::new(0)),
        }
    }

    pub fn add_data_exchanged_callback(&self, cb: FsoeDataExchangeCallback) {
        if let Ok(mut list) = self.callbacks.lock() {
            list.push(cb);
        }
    }

    pub fn remove_data_exchanged_callback(&self, cb: &FsoeDataExchangeCallback) {
        if let Ok(mut list) = self.callbacks.lock() {
            list.retain(|c| !Arc::ptr_eq(c, cb));
        }
    }

    pub fn fire_data_exchanged(
        &self,
        slave_index: u16,
        current_state: FsoeState,
        safe_inputs: &[u8],
        safe_outputs: &[u8],
        in_failsafe: bool,
    ) {
        let cycle = {
            let mut c = match self.cycle_count.lock() {
                Ok(g) => g,
                Err(_) => return,
            };
            *c = c.wrapping_add(1);
            *c
        };

        let snapshot: Vec<FsoeDataExchangeCallback> = match self.callbacks.lock() {
            Ok(list) => list.clone(),
            Err(_) => return,
        };

        if snapshot.is_empty() {
            return;
        }

        let args = FsoeDataExchangeEventArgs {
            slave_index,
            current_state,
            safe_inputs: safe_inputs.to_vec(),
            safe_outputs: safe_outputs.to_vec(),
            in_failsafe,
            cycle_count: cycle,
            timestamp: SystemTime::now(),
        };

        for cb in &snapshot {

            let _ = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| cb(&args)));
        }
    }
}

impl Default for FsoeDataExchangeDispatcher {
    fn default() -> Self {
        Self::new()
    }
}

pub struct FsoeEventCallbacks {

    pub on_state_changed: Option<Box<dyn Fn(&FsoeStateChangedEventArgs) + Send + Sync>>,

    pub on_error: Option<Box<dyn Fn(&FsoeErrorEventArgs) + Send + Sync>>,

    pub on_failsafe: Option<Box<dyn Fn(&FsoeFailsafeEventArgs) + Send + Sync>>,

    pub on_safe_data_updated: Option<Box<dyn Fn(&FsoeDataUpdatedEventArgs) + Send + Sync>>,
}

impl FsoeEventCallbacks {

    pub fn new() -> Self {
        Self {
            on_state_changed: None,
            on_error: None,
            on_failsafe: None,
            on_safe_data_updated: None,
        }
    }

    pub fn set_on_state_changed<F>(&mut self, f: F)
    where
        F: Fn(&FsoeStateChangedEventArgs) + Send + Sync + 'static,
    {
        self.on_state_changed = Some(Box::new(f));
    }

    pub fn set_on_error<F>(&mut self, f: F)
    where
        F: Fn(&FsoeErrorEventArgs) + Send + Sync + 'static,
    {
        self.on_error = Some(Box::new(f));
    }

    pub fn set_on_failsafe<F>(&mut self, f: F)
    where
        F: Fn(&FsoeFailsafeEventArgs) + Send + Sync + 'static,
    {
        self.on_failsafe = Some(Box::new(f));
    }

    pub fn set_on_safe_data_updated<F>(&mut self, f: F)
    where
        F: Fn(&FsoeDataUpdatedEventArgs) + Send + Sync + 'static,
    {
        self.on_safe_data_updated = Some(Box::new(f));
    }

    pub fn fire_state_changed(&self, args: &FsoeStateChangedEventArgs) {
        if let Some(ref cb) = self.on_state_changed {
            cb(args);
        }
    }

    pub fn fire_error(&self, args: &FsoeErrorEventArgs) {
        if let Some(ref cb) = self.on_error {
            cb(args);
        }
    }

    pub fn fire_failsafe(&self, args: &FsoeFailsafeEventArgs) {
        if let Some(ref cb) = self.on_failsafe {
            cb(args);
        }
    }

    pub fn fire_safe_data_updated(&self, args: &FsoeDataUpdatedEventArgs) {
        if let Some(ref cb) = self.on_safe_data_updated {
            cb(args);
        }
    }
}

impl Default for FsoeEventCallbacks {
    fn default() -> Self {
        Self::new()
    }
}

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

    pub version: [u8; 2],

    pub safety_address: u16,

    pub connection_id: u16,

    pub watchdog_time: u32,

    pub unique_device_id: [u8; 6],

    pub connection_type: u16,

    pub comm_param_length: u16,

    pub appl_param_length: u16,

    pub sra_crc: u32,
}

impl Default for SafeModuleCommParam {
    fn default() -> Self {
        Self {
            version: [0; 2],
            safety_address: 0,
            connection_id: 0,
            watchdog_time: 0,
            unique_device_id: [0; 6],
            connection_type: 0,
            comm_param_length: 0,
            appl_param_length: 0,
            sra_crc: 0,
        }
    }
}

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

    pub connection_state: u16,

    pub connection_diagnosis: u16,
}

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

    pub connection_id: u16,

    pub device_type: u16,

    pub vendor_id: u32,

    pub product_code: u32,

    pub revision_number: u32,

    pub serial_number: u32,
}

pub struct SafeMdpIndex;

impl SafeMdpIndex {

    pub const AXIS_INDEX_OFFSET: u16 = 0x0800;

    pub const CONFIG_AREA_BASE: u16 = 0x8000;

    pub const DEVICE_FSOE_ADDRESS: u16 = 0xF980;

    pub const DIAG_AREA_BASE: u16 = 0xA000;

    pub const DRIVE_CONFIG_AREA_BASE: u16 = 0xE800;

    pub const DRIVE_DIAG_AREA_BASE: u16 = 0xEA00;

    pub const DRIVE_INFO_AREA_BASE: u16 = 0xE900;

    pub const DRIVE_INPUT_AREA_BASE: u16 = 0xE600;

    pub const DRIVE_OUTPUT_AREA_BASE: u16 = 0xE700;

    pub const INFO_AREA_BASE: u16 = 0x9000;

    pub const INPUT_AREA_BASE: u16 = 0x6000;

    pub const MODULE_INDEX_OFFSET: u16 = 0x0010;

    pub const OUTPUT_AREA_BASE: u16 = 0x7000;

    pub const PARAM_SET_MAPPING_BASE: u16 = 0x1E00;

    pub const RX_PDO_MAPPING_BASE: u16 = 0x1600;

    pub const TX_PDO_MAPPING_BASE: u16 = 0x1A00;

    pub fn get_axis_index(base_index: u16, axis_number: u16) -> u16 {
        base_index + axis_number * Self::AXIS_INDEX_OFFSET
    }

    pub fn get_module_index(base_index: u16, module_number: u16) -> u16 {
        base_index + module_number * Self::MODULE_INDEX_OFFSET
    }
}

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

    pub module_number: u16,

    pub axis_number: u16,

    pub profile: SafeModuleProfile,

    pub safety_address: u16,

    pub connection_id: u16,

    pub connection_type: FsoeConnectionType,

    pub safe_input_size: u16,

    pub safe_output_size: u16,

    pub pdo_input_offset: u32,

    pub pdo_output_offset: u32,

    pub watchdog_time_ms: u32,
}

impl Default for SafeModuleConfig {
    fn default() -> Self {
        Self {
            module_number: 0,
            axis_number: 0,
            profile: SafeModuleProfile::DigitalInOut,
            safety_address: 0,
            connection_id: 0,
            connection_type: FsoeConnectionType::Slave,
            safe_input_size: 0,
            safe_output_size: 0,
            pdo_input_offset: 0,
            pdo_output_offset: 0,
            watchdog_time_ms: 100,
        }
    }
}

impl SafeModuleConfig {

    pub fn to_connection_config(&self) -> FsoeConnectionConfig {
        FsoeConnectionConfig {
            connection_id: self.connection_id,
            safety_address: self.safety_address,
            watchdog_time_ms: self.watchdog_time_ms,
            safe_input_size: self.safe_input_size,
            safe_output_size: self.safe_output_size,
            pdo_input_offset: self.pdo_input_offset,
            pdo_output_offset: self.pdo_output_offset,
        }
    }
}

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

    pub connection_mode: FsoeConnectionMode,

    pub device_safety_address: u16,

    pub is_drive: bool,

    pub axis_count: u16,

    pub modules: Vec<SafeModuleConfig>,
}

impl Default for SafeMdpDeviceConfig {
    fn default() -> Self {
        Self {
            connection_mode: FsoeConnectionMode::Single,
            device_safety_address: 0,
            is_drive: false,
            axis_count: 1,
            modules: Vec::new(),
        }
    }
}

impl SafeMdpDeviceConfig {

    pub fn add_module(&mut self, profile: SafeModuleProfile, safety_address: u16) -> &mut SafeModuleConfig {
        let idx = self.modules.len() as u16;
        self.modules.push(SafeModuleConfig {
            module_number: idx,
            profile,
            safety_address,
            connection_id: 0x0001 + idx,
            ..Default::default()
        });
        self.modules.last_mut().unwrap()
    }

    pub fn add_drive_axis(&mut self, axis_number: u16, safety_address: u16) -> &mut SafeModuleConfig {
        let idx = self.modules.len() as u16;
        self.modules.push(SafeModuleConfig {
            module_number: idx,
            axis_number,
            profile: SafeModuleProfile::DriveConnection,
            safety_address,
            connection_id: 0x0001 + idx,
            ..Default::default()
        });
        self.modules.last_mut().unwrap()
    }

    pub fn get_connection_count(&self) -> usize {
        match self.connection_mode {
            FsoeConnectionMode::Single => 1,
            FsoeConnectionMode::Multiple => self.modules.len(),
        }
    }
}

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

    pub command_offset: usize,

    pub flags_offset: usize,

    pub connection_id_offset: usize,

    pub sequence_offset: usize,

    pub data_offset: usize,

    pub data_length: usize,

    pub crc_length: usize,
}

impl Default for FsoePdoLayout {
    fn default() -> Self {
        Self {
            command_offset: 0,
            flags_offset: 1,
            connection_id_offset: 2,
            sequence_offset: 4,
            data_offset: 6,
            data_length: 0,
            crc_length: 2,
        }
    }
}

impl FsoePdoLayout {

    pub fn total_length(&self) -> usize {
        self.data_offset + self.data_length + self.crc_length
    }

    pub fn validate(&self) -> Result<()> {
        if self.data_offset < self.sequence_offset {
            return Err(DarraError::Other(
                "DataOffset 必须大于等于 SequenceOffset".to_string(),
            ));
        }
        if self.crc_length == 0 {
            return Err(DarraError::Other(
                "CRC 长度必须大于 0".to_string(),
            ));
        }
        Ok(())
    }
}

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

    pub command: FsoeCommand,

    pub flags: u8,

    pub connection_id: u16,

    pub sequence: u16,

    pub data: Vec<u8>,

    pub crc: u16,
}

impl Default for FsoePdoFrame {
    fn default() -> Self {
        Self {
            command: FsoeCommand::Reset,
            flags: 0,
            connection_id: 0,
            sequence: 0,
            data: Vec::new(),
            crc: 0,
        }
    }
}

impl FsoePdoFrame {

    pub fn to_bytes(&mut self, layout: &FsoePdoLayout, crc_func: Option<fn(&[u8]) -> u16>) -> Result<Vec<u8>> {
        layout.validate()?;
        if self.data.len() != layout.data_length {
            return Err(DarraError::Other(format!(
                "Data 长度必须为 {} 字节，但收到 {}",
                layout.data_length,
                self.data.len()
            )));
        }

        let total = layout.total_length();
        let mut buf = vec![0u8; total];
        buf[layout.command_offset] = self.command as u8;
        buf[layout.flags_offset] = self.flags;
        buf[layout.connection_id_offset] = (self.connection_id & 0xFF) as u8;
        buf[layout.connection_id_offset + 1] = ((self.connection_id >> 8) & 0xFF) as u8;
        buf[layout.sequence_offset] = (self.sequence & 0xFF) as u8;
        buf[layout.sequence_offset + 1] = ((self.sequence >> 8) & 0xFF) as u8;
        buf[layout.data_offset..layout.data_offset + layout.data_length]
            .copy_from_slice(&self.data);

        if let Some(f) = crc_func {
            self.crc = f(&buf[..total - layout.crc_length]);
        }

        let crc_offset = layout.data_offset + layout.data_length;
        buf[crc_offset] = (self.crc & 0xFF) as u8;
        buf[crc_offset + 1] = ((self.crc >> 8) & 0xFF) as u8;
        Ok(buf)
    }

    pub fn from_bytes(data: &[u8], layout: &FsoePdoLayout, crc_func: Option<fn(&[u8]) -> u16>) -> Result<Self> {
        layout.validate()?;
        let total = layout.total_length();
        if data.len() < total {
            return Err(DarraError::Other(format!(
                "数据长度不足，期望 {} 字节，实际 {}",
                total,
                data.len()
            )));
        }

        let command = match data[layout.command_offset] {
            0x2A => FsoeCommand::Reset,
            0x4E => FsoeCommand::Session,
            0x64 => FsoeCommand::Connection,
            0x52 => FsoeCommand::Parameter,
            0x08 => FsoeCommand::Failsafe,
            0x36 => FsoeCommand::Data,
            v => return Err(DarraError::Other(format!("无效的 FSoE 命令: 0x{:02X}", v))),
        };
        let flags = data[layout.flags_offset];
        let connection_id = u16::from_le_bytes([
            data[layout.connection_id_offset],
            data[layout.connection_id_offset + 1],
        ]);
        let sequence = u16::from_le_bytes([
            data[layout.sequence_offset],
            data[layout.sequence_offset + 1],
        ]);
        let frame_data = data[layout.data_offset..layout.data_offset + layout.data_length].to_vec();
        let crc_offset = layout.data_offset + layout.data_length;
        let crc = u16::from_le_bytes([data[crc_offset], data[crc_offset + 1]]);

        if let Some(f) = crc_func {
            let computed = f(&data[..total - layout.crc_length]);
            if computed != crc {
                return Err(DarraError::Other(format!(
                    "FSoE CRC 校验失败：期望 0x{:04X}，计算值 0x{:04X}",
                    crc, computed
                )));
            }
        }

        Ok(Self {
            command,
            flags,
            connection_id,
            sequence,
            data: frame_data,
            crc,
        })
    }
}

pub struct FsoeCrc16Configurable {
    polynomial: u16,
    initial: u16,
    xor_out: u16,
}

impl FsoeCrc16Configurable {

    pub fn new(polynomial: u16, initial: u16, xor_out: u16) -> Self {
        Self { polynomial, initial, xor_out }
    }

    pub fn compute(&self, data: &[u8]) -> u16 {
        let mut crc = self.initial;
        for &b in data {
            crc ^= (b as u16) << 8;
            for _ in 0..8 {
                if (crc & 0x8000) != 0 {
                    crc = (crc << 1) ^ self.polynomial;
                } else {
                    crc <<= 1;
                }
            }
        }
        (crc ^ self.xor_out) & 0xFFFF
    }
}

impl Default for FsoeCrc16Configurable {
    fn default() -> Self {
        Self::new(0x1021, 0xFFFF, 0x0000)
    }
}

pub fn fsoe_crc16_ccitt_false() -> FsoeCrc16Configurable {
    FsoeCrc16Configurable::new(0x1021, 0xFFFF, 0x0000)
}

pub fn fsoe_crc16(data: &[u8]) -> u16 {
    FsoeCrc16::compute(data)
}

pub fn fsoe_crc16_fast(data: &[u8]) -> u16 {
    FsoeCrc16::compute(data)
}

pub fn validate_conn_id(conn_id: u16) -> bool {
    unsafe { ffi::FSoEValidateConnId(conn_id) != 0 }
}

pub fn is_connection_id_available(conn_id: u16) -> bool {
    validate_conn_id(conn_id)
}

impl crate::abstractions::MailboxProtocol for SafeMdp {
    fn protocol_type(&self) -> u8 { 0x08 }
    fn protocol_name(&self) -> &'static str { "FSoE" }

    fn is_supported(&self) -> bool {
        self.detect_connections() > 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, 0x08, &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, 0x08);
        }
    }
}
