using CanTest;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;

namespace CCDCount.DLL.CanBus
{
    /// <summary>
    /// CANopen从站 - 基于创芯科技CAN卡底层库实现
    /// 支持SDO、PDO、NMT、心跳等标准CANopen从站功能
    /// </summary>
    public class CanOpenSlave : IDisposable
    {
        #region 私有字段
        
        private CanOpenManager m_canManager;
        private ObjectDictionary m_objectDict;
        private byte m_nodeId;
        private NmtState m_currentState = NmtState.Initializing;
        private bool m_isRunning = false;
        private Thread m_receiveThread;
        private CancellationTokenSource m_cancellationTokenSource;
        
        // PDO配置
        private PdoConfig[] m_tpdoConfigs = new PdoConfig[4];
        private PdoConfig[] m_rpdoConfigs = new PdoConfig[4];
        
        // 心跳配置
        private ushort m_heartbeatTime = 1000; // 毫秒
        private DateTime m_lastHeartbeatTime = DateTime.MinValue; // 初始化为最小值，确保首次立即发送
        
        
        #endregion
        
        #region 构造函数和初始化
        
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="nodeId">节点ID(1-127)</param>
        /// <param name="deviceType">设备类型</param>
        /// <param name="deviceIndex">设备索引</param>
        /// <param name="canIndex">CAN通道索引</param>
        public CanOpenSlave(byte nodeId, UInt32 deviceType = 4, UInt32 deviceIndex = 0, UInt32 canIndex = 0)
        {
            if (nodeId < 1 || nodeId > 127)
                throw new ArgumentException("节点ID必须在1-127之间");
            
            m_nodeId = nodeId;
            m_canManager = new CanOpenManager(deviceType, deviceIndex, canIndex);
            m_objectDict = new ObjectDictionary(nodeId);
            InitializePdoConfigs();
        }
        
        private void InitializePdoConfigs()
        {
            for (int i = 0; i < 4; i++)
            {
                m_tpdoConfigs[i] = new PdoConfig { Enabled = false, TransmissionType = 255 };
                m_rpdoConfigs[i] = new PdoConfig { Enabled = false };
            }
        }
        
        /// <summary>
        /// 初始化从站
        /// </summary>
        /// <param name="baudRate">波特率</param>
        /// <returns>是否成功</returns>
        public bool Initialize(CanBaudRate baudRate = CanBaudRate.BaudRate_500K)
        {
            if (!m_canManager.Initialize(baudRate))
                return false;
            
            // 设置初始状态为预操作
            m_currentState = NmtState.PreOperational;
            
            // 立即发送第一个心跳，让PLC能够快速发现设备
            SendHeartbeatImmediately();
            
            Console.WriteLine($"CANopen从站初始化完成 - 节点ID: {m_nodeId}");
            
            return true;
        }
        
        /// <summary>
        /// 立即发送心跳报文（用于设备发现）
        /// </summary>
        private void SendHeartbeatImmediately()
        {
            byte statusByte = GetNodeStatusByte();
            byte[] heartbeatData = new byte[] { statusByte };
            uint heartbeatCobId = (uint)(0x700 + m_nodeId);
            m_canManager.SendCanFrame(heartbeatCobId, heartbeatData);
            m_lastHeartbeatTime = DateTime.Now;
        }
        
        /// <summary>
        /// 启动从站
        /// </summary>
        public void Start()
        {
            if (m_isRunning)
                return;
            
            m_isRunning = true;
            m_cancellationTokenSource = new CancellationTokenSource();
            m_receiveThread = new Thread(ReceiveLoop);
            m_receiveThread.IsBackground = true;
            m_receiveThread.Start();
            
            Console.WriteLine("CANopen从站已启动");
        }
        
        /// <summary>
        /// 停止从站
        /// </summary>
        public void Stop()
        {
            m_isRunning = false;
            m_cancellationTokenSource?.Cancel();
            m_receiveThread?.Join(1000);
            Console.WriteLine("CANopen从站已停止");
        }
        
        #endregion
        
        #region 接收处理线程
        
        private void ReceiveLoop()
        {
            while (m_isRunning && !m_cancellationTokenSource.Token.IsCancellationRequested)
            {
                try
                {
                    var frames = m_canManager.ReceiveCanFrames(50);
                    
                    foreach (var frame in frames)
                    {
                        ProcessCanFrame(frame);
                    }
                    
                    // 发送心跳
                    SendHeartbeat();
                    
                    Thread.Sleep(1);
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"接收线程异常: {ex.Message}");
                }
            }
        }
        
        private void ProcessCanFrame(CanOpenFrame frame)
        {
            uint cobId = frame.CobId;
            
            // NMT消息处理(COB-ID = 0x000)
            if (cobId == 0x000)
            {
                ProcessNmtMessage(frame);
                return;
            }
            
            // SDO请求处理
            if ((cobId & 0x780) == 0x600)
            {
                byte remoteNodeId = (byte)(cobId & 0x7F);
                if (remoteNodeId == m_nodeId)
                {
                    ProcessSdoRequest(frame);
                }
                return;
            }
            
            // RPDO处理
            if ((cobId & 0x780) >= 0x200 && (cobId & 0x780) <= 0x500)
            {
                ProcessRpdo(frame);
                return;
            }
        }
        
        #endregion
        
        #region NMT处理
        
        private void ProcessNmtMessage(CanOpenFrame frame)
        {
            if (frame.DataLength < 2)
                return;
            
            byte command = frame.Data[0];
            byte nodeId = frame.Data[1];
            
            // 0表示广播到所有节点
            if (nodeId != 0 && nodeId != m_nodeId)
                return;
            
            switch (command)
            {
                case 0x01: // 启动节点
                    if (m_currentState == NmtState.PreOperational || m_currentState == NmtState.Stopped)
                    {
                        m_currentState = NmtState.Operational;
                        Console.WriteLine($"NMT: 节点{m_nodeId}进入运行状态");
                    }
                    break;
                    
                case 0x02: // 停止节点
                    if (m_currentState == NmtState.Operational)
                    {
                        m_currentState = NmtState.Stopped;
                        Console.WriteLine($"NMT: 节点{m_nodeId}进入停止状态");
                    }
                    break;
                    
                case 0x80: // 进入预操作
                    m_currentState = NmtState.PreOperational;
                    Console.WriteLine($"NMT: 节点{m_nodeId}进入预操作状态");
                    break;
                    
                case 0x81: // 重置节点
                    ResetNode();
                    break;
                    
                case 0x82: // 重置通信
                    ResetCommunication();
                    break;
            }
        }
        
        private void ResetNode()
        {
            Console.WriteLine($"NMT: 节点{m_nodeId}重置");
            m_currentState = NmtState.Initializing;
            Thread.Sleep(100);
            m_currentState = NmtState.PreOperational;
        }
        
        private void ResetCommunication()
        {
            Console.WriteLine($"NMT: 节点{m_nodeId}重置通信");
            // 重置PDO配置等通信参数
            InitializePdoConfigs();
            m_currentState = NmtState.PreOperational;
        }
        
        #endregion
        
        #region SDO处理
        
        private void ProcessSdoRequest(CanOpenFrame frame)
        {
            if (frame.DataLength < 8)
                return;
            
            byte sdoCommand = frame.Data[0];
            ushort index = (ushort)(frame.Data[1] | (frame.Data[2] << 8));
            byte subIndex = frame.Data[3];
            
            // 判断是读取还是写入
            if ((sdoCommand & 0xE0) == 0x40) // SDO上传请求(读)
            {
                HandleSdoRead(index, subIndex);
            }
            else if ((sdoCommand & 0xE0) == 0x20) // SDO下载请求(写)
            {
                HandleSdoWrite(sdoCommand, index, subIndex, frame.Data);
            }
        }
        
        private void HandleSdoRead(ushort index, byte subIndex)
        {
            byte[] responseData = new byte[8];
            
            // 从对象字典读取数据
            object value = m_objectDict.Read(index, subIndex);
            
            if (value == null)
            {
                // 对象不存在，返回错误
                responseData[0] = 0x80; //  abort transfer
                responseData[1] = (byte)(index & 0xFF);
                responseData[2] = (byte)(index >> 8);
                responseData[3] = subIndex;
                responseData[4] = 0x06; // 对象不存在
                responseData[5] = 0x09;
                responseData[6] = 0x00;
                responseData[7] = 0x00;
                
                SendSdoResponse(responseData);
                Console.WriteLine($"SDO读失败: 0x{index:X4}sub{subIndex} 不存在");
                return;
            }
            
            // 特殊处理字符串类型（设备名称、版本等）
            if (value is string strVal)
            {
                // 对于可见字符串，需要分段传输或使用快速上传
                byte[] strBytes = Encoding.ASCII.GetBytes(strVal);
                int maxLen = Math.Min(strBytes.Length, 4); // 最多4字节用于快速上传
                
                responseData[0] = (byte)(0x4F | ((4 - maxLen) << 2)); // 快速上传，指定有效字节数
                responseData[1] = (byte)(index & 0xFF);
                responseData[2] = (byte)(index >> 8);
                responseData[3] = subIndex;
                
                for (int i = 0; i < maxLen; i++)
                {
                    responseData[4 + i] = strBytes[i];
                }
                
                SendSdoResponse(responseData);
                Console.WriteLine($"SDO读字符串: 0x{index:X4}sub{subIndex} = {strVal.Substring(0, maxLen)}...");
                return;
            }
            
            // 根据数据类型构造响应
            if (value is byte byteVal)
            {
                responseData[0] = 0x4F; // 快速上传，1字节
                responseData[1] = (byte)(index & 0xFF);
                responseData[2] = (byte)(index >> 8);
                responseData[3] = subIndex;
                responseData[4] = byteVal;
            }
            else if (value is ushort ushortVal)
            {
                responseData[0] = 0x4B; // 快速上传，2字节
                responseData[1] = (byte)(index & 0xFF);
                responseData[2] = (byte)(index >> 8);
                responseData[3] = subIndex;
                responseData[4] = (byte)(ushortVal & 0xFF);
                responseData[5] = (byte)(ushortVal >> 8);
            }
            else if (value is uint uintVal)
            {
                responseData[0] = 0x43; // 快速上传，4字节
                responseData[1] = (byte)(index & 0xFF);
                responseData[2] = (byte)(index >> 8);
                responseData[3] = subIndex;
                responseData[4] = (byte)(uintVal & 0xFF);
                responseData[5] = (byte)((uintVal >> 8) & 0xFF);
                responseData[6] = (byte)((uintVal >> 16) & 0xFF);
                responseData[7] = (byte)((uintVal >> 24) & 0xFF);
            }
            else
            {
                // 不支持的类型
                responseData[0] = 0x80;
                responseData[4] = 0x06; // 对象不存在
                Console.WriteLine($"SDO读失败: 0x{index:X4}sub{subIndex} 类型不支持");
            }
            
            SendSdoResponse(responseData);
        }
        
        private void HandleSdoWrite(byte command, ushort index, byte subIndex, byte[] data)
        {
            byte[] responseData = new byte[8];
            bool success = false;
            
            // 解析写入的值
            uint writeValue = 0;
            
            if ((command & 0x03) == 0x01) // 1字节
            {
                writeValue = data[4];
            }
            else if ((command & 0x03) == 0x02) // 2字节
            {
                writeValue = (uint)(data[4] | (data[5] << 8));
            }
            else if ((command & 0x03) == 0x00) // 4字节
            {
                writeValue = (uint)(data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24));
            }
            
            // 写入对象字典
            success = m_objectDict.Write(index, subIndex, writeValue);
            
            if (success)
            {
                // 发送确认响应
                responseData[0] = 0x60;
                responseData[1] = (byte)(index & 0xFF);
                responseData[2] = (byte)(index >> 8);
                responseData[3] = subIndex;
                SendSdoResponse(responseData);
            }
            else
            {
                // 发送错误响应
                responseData[0] = 0x80;
                responseData[1] = (byte)(index & 0xFF);
                responseData[2] = (byte)(index >> 8);
                responseData[3] = subIndex;
                responseData[4] = 0x06; // 对象不存在或只读
                responseData[5] = 0x09;
                responseData[6] = 0x00;
                responseData[7] = 0x00;
                SendSdoResponse(responseData);
            }
        }
        
        private void SendSdoResponse(byte[] data)
        {
            uint responseCobId = (uint)(0x580 + m_nodeId);
            m_canManager.SendCanFrame(responseCobId, data);
        }
        
        #endregion
        
        #region PDO处理
        
        private void ProcessRpdo(CanOpenFrame frame)
        {
            if (m_currentState != NmtState.Operational)
                return;
            
            // 解析COB-ID确定是哪个RPDO
            uint baseId = frame.CobId & 0x780;
            byte pdoNumber = 0;
            
            if (baseId == 0x200) pdoNumber = 1;
            else if (baseId == 0x300) pdoNumber = 2;
            else if (baseId == 0x400) pdoNumber = 3;
            else if (baseId == 0x500) pdoNumber = 4;
            
            if (pdoNumber == 0)
                return;
            
            int configIndex = pdoNumber - 1;
            if (!m_rpdoConfigs[configIndex].Enabled)
                return;
            
            // 将RPDO数据映射到对象字典
            MapRpdoToObjectDict(pdoNumber, frame.Data, frame.DataLength);
            
            Console.WriteLine($"收到RPDO{pdoNumber}: 节点{m_nodeId}, 长度{frame.DataLength}");
        }
        
        private void MapRpdoToObjectDict(byte pdoNumber, byte[] data, byte length)
        {
            // 根据配置的映射关系将数据写入对象字典
            // 这里简化处理，实际应用中需要根据具体映射配置
            ushort mappingIndex = (ushort)(0x1600 + pdoNumber - 1);
            
            // 示例：假设第一个映射项是0x6000:00
            object mappingEntry = m_objectDict.Read(mappingIndex, 0x01);
            if (mappingEntry != null && mappingEntry is uint mapping)
            {
                ushort objIndex = (ushort)(mapping >> 16);
                byte objSubIndex = (byte)(mapping & 0xFF);
                
                if (length >= 1)
                {
                    m_objectDict.Write(objIndex, objSubIndex, data[0]);
                }
            }
        }
        
        /// <summary>
        /// 发送TPDO
        /// </summary>
        public void SendTpdo(byte pdoNumber)
        {
            if (m_currentState != NmtState.Operational)
                return;
            
            if (pdoNumber < 1 || pdoNumber > 4)
                return;
            
            int configIndex = pdoNumber - 1;
            if (!m_tpdoConfigs[configIndex].Enabled)
                return;
            
            // 从对象字典读取数据并打包
            byte[] tpdoData = PackTpdoData(pdoNumber);
            
            if (tpdoData != null && tpdoData.Length > 0)
            {
                m_canManager.SendTpdo(m_nodeId, pdoNumber, tpdoData);
            }
        }
        
        private byte[] PackTpdoData(byte pdoNumber)
        {
            List<byte> data = new List<byte>();
            ushort mappingIndex = (ushort)(0x1A00 + pdoNumber - 1);
            
            // 读取映射条目数量
            object entryCountObj = m_objectDict.Read(mappingIndex, 0x00);
            if (entryCountObj == null)
                return null;
            
            byte entryCount = Convert.ToByte(entryCountObj);
            
            // 遍历所有映射项
            for (byte i = 1; i <= entryCount; i++)
            {
                object mappingObj = m_objectDict.Read(mappingIndex, i);
                if (mappingObj == null)
                    continue;
                
                uint mapping = Convert.ToUInt32(mappingObj);
                ushort objIndex = (ushort)(mapping >> 16);
                byte objSubIndex = (byte)((mapping >> 8) & 0xFF);
                byte dataSize = (byte)(mapping & 0xFF);
                
                object value = m_objectDict.Read(objIndex, objSubIndex);
                if (value == null)
                    continue;
                
                // 根据数据大小添加字节
                if (dataSize == 8 && value is byte byteVal)
                {
                    data.Add(byteVal);
                }
                else if (dataSize == 16 && value is ushort ushortVal)
                {
                    data.Add((byte)(ushortVal & 0xFF));
                    data.Add((byte)(ushortVal >> 8));
                }
                else if (dataSize == 32 && value is uint uintVal)
                {
                    data.Add((byte)(uintVal & 0xFF));
                    data.Add((byte)((uintVal >> 8) & 0xFF));
                    data.Add((byte)((uintVal >> 16) & 0xFF));
                    data.Add((byte)((uintVal >> 24) & 0xFF));
                }
            }
            
            return data.ToArray();
        }
        
        #endregion
        
        #region 心跳机制
        
        private void SendHeartbeat()
        {
            if (m_heartbeatTime == 0)
                return;
            
            TimeSpan elapsed = DateTime.Now - m_lastHeartbeatTime;
            if (elapsed.TotalMilliseconds >= m_heartbeatTime)
            {
                byte statusByte = GetNodeStatusByte();
                byte[] heartbeatData = new byte[] { statusByte };
                
                uint heartbeatCobId = (uint)(0x700 + m_nodeId);
                m_canManager.SendCanFrame(heartbeatCobId, heartbeatData);
                
                m_lastHeartbeatTime = DateTime.Now;
            }
        }
        
        private byte GetNodeStatusByte()
        {
            switch (m_currentState)
            {
                case NmtState.Initializing:
                    return 0x00;
                case NmtState.PreOperational:
                    return 0x7F;
                case NmtState.Operational:
                    return 0x05;
                case NmtState.Stopped:
                    return 0x04;
                default:
                    return 0x7F;
            }
        }
        
        /// <summary>
        /// 设置心跳时间
        /// </summary>
        public void SetHeartbeatTime(ushort milliseconds)
        {
            m_heartbeatTime = milliseconds;
            m_objectDict.Write(0x1017, 0x00, milliseconds);
        }
        
        #endregion
        
        #region 公共API
        
        /// <summary>
        /// 设置对象字典值
        /// </summary>
        public bool SetObjectValue(ushort index, byte subIndex, object value)
        {
            return m_objectDict.Write(index, subIndex, value);
        }
        
        /// <summary>
        /// 获取对象字典值
        /// </summary>
        public object GetObjectValue(ushort index, byte subIndex)
        {
            return m_objectDict.Read(index, subIndex);
        }
        
        /// <summary>
        /// 配置TPDO
        /// </summary>
        public void ConfigureTpdo(byte pdoNumber, bool enabled, byte transmissionType = 255)
        {
            if (pdoNumber < 1 || pdoNumber > 4)
                return;
            
            int index = pdoNumber - 1;
            m_tpdoConfigs[index].Enabled = enabled;
            m_tpdoConfigs[index].TransmissionType = transmissionType;
            
            // 更新对象字典
            ushort commIndex = (ushort)(0x1800 + index);
            m_objectDict.Write(commIndex, 0x01, enabled ? (uint)(0x180 + m_nodeId + index * 0x100) : (uint)(0x180 + m_nodeId + index * 0x100 | 0x80000000));
            m_objectDict.Write(commIndex, 0x02, transmissionType);
        }
        
        /// <summary>
        /// 配置RPDO
        /// </summary>
        public void ConfigureRpdo(byte pdoNumber, bool enabled)
        {
            if (pdoNumber < 1 || pdoNumber > 4)
                return;
            
            int index = pdoNumber - 1;
            m_rpdoConfigs[index].Enabled = enabled;
            
            // 更新对象字典
            ushort commIndex = (ushort)(0x1400 + index);
            m_objectDict.Write(commIndex, 0x01, enabled ? (uint)(0x200 + m_nodeId + index * 0x100) : (uint)(0x200 + m_nodeId + index * 0x100 | 0x80000000));
        }
        
        /// <summary>
        /// 获取当前NMT状态
        /// </summary>
        public NmtState GetCurrentState()
        {
            return m_currentState;
        }
        
        /// <summary>
        /// 生成EDS文件
        /// </summary>
        public string GenerateEdsFile(string filePath = null)
        {
            var edsGenerator = new EdsFileGenerator(m_nodeId);
            return edsGenerator.Generate(m_objectDict, filePath);
        }
        
        #endregion
        
        #region 资源管理
        
        public void Dispose()
        {
            Stop();
            m_canManager?.Dispose();
        }
        
        #endregion
    }
    
    #region 辅助类
    
    /// <summary>
    /// NMT状态枚举
    /// </summary>
    public enum NmtState
    {
        Initializing,
        PreOperational,
        Operational,
        Stopped
    }
    
    /// <summary>
    /// PDO配置
    /// </summary>
    public class PdoConfig
    {
        public bool Enabled { get; set; }
        public byte TransmissionType { get; set; }
        public uint CobId { get; set; }
    }
    
    #endregion
}