using System.Collections.Concurrent;
using System.Text.RegularExpressions;
using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen.Enums;
using RobotNet10.CANOpen.Exceptions;
using RobotNet10.CANOpen.Interfaces;
using RobotNet10.CANOpen.Models;

namespace RobotNet10.CANOpen.Services;

/// <summary>
/// PDO Manager để xử lý Transmit và Receive PDOs
/// </summary>
public class PdoManager : IDisposable
{
    private readonly ICanBus _canBus;
    private readonly byte _nodeId;
    private readonly ILogger<PdoManager>? _logger;
    private readonly ConcurrentDictionary<byte, PdoConfiguration> _tpdoConfigs;
    private readonly ConcurrentDictionary<byte, PdoConfiguration> _rpdoConfigs;
    private readonly HashSet<byte> _configuredTpdoNumbers = new(); // Track TPDO đã configure thành công
    private readonly HashSet<byte> _configuredRpdoNumbers = new(); // Track RPDO đã configure thành công
    private readonly ConcurrentDictionary<byte, long> _lastUnconfiguredTpdoWarningTicks = new(); // Throttle repeated warnings
    private const int UnconfiguredTpdoWarningIntervalMs = 60000; // Log at most once per minute per TPDO
    private bool _disposed;
    
    public event EventHandler<PdoReceivedEventArgs>? PdoReceived;
    
    public PdoManager(ICanBus canBus, byte nodeId, ILogger<PdoManager>? logger = null)
    {
        _canBus = canBus ?? throw new ArgumentNullException(nameof(canBus));
        _nodeId = nodeId;
        _logger = logger;
        _tpdoConfigs = new ConcurrentDictionary<byte, PdoConfiguration>();
        _rpdoConfigs = new ConcurrentDictionary<byte, PdoConfiguration>();
        
        _canBus.FrameReceived += OnFrameReceived;
    }
    
    /// <summary>
    /// Check xem TPDO đã được configure thành công trên device chưa
    /// </summary>
    public bool IsTpdoConfigured(byte pdoNumber) => _configuredTpdoNumbers.Contains(pdoNumber);
    
    /// <summary>
    /// Check xem RPDO đã được configure thành công trên device chưa
    /// </summary>
    public bool IsRpdoConfigured(byte pdoNumber) => _configuredRpdoNumbers.Contains(pdoNumber);
    
    /// <summary>
    /// Configure TPDO (Transmit PDO - từ device)
    /// </summary>
    public void ConfigureTPDO(PdoConfiguration config)
    {
        if (_disposed)
            throw new ObjectDisposedException(nameof(PdoManager));
        
        _tpdoConfigs[config.PdoNumber] = config;
    }
    
    /// <summary>
    /// Configure RPDO (Receive PDO - đến device)
    /// </summary>
    public void ConfigureRPDO(PdoConfiguration config)
    {
        if (_disposed)
            throw new ObjectDisposedException(nameof(PdoManager));
        
        _rpdoConfigs[config.PdoNumber] = config;
    }
    
    /// <summary>
    /// Gửi RPDO (Receive PDO) đến device
    /// Tự động fallback về SDO nếu PDO chưa được configure trên device
    /// </summary>
    public async Task SendRPDOAsync(byte pdoNumber, byte[] data, CancellationToken ct = default)
    {
        if (_disposed)
            throw new ObjectDisposedException(nameof(PdoManager));
        
        if (!_rpdoConfigs.TryGetValue(pdoNumber, out var config))
            throw new InvalidOperationException($"RPDO {pdoNumber} not configured in master-side");
        
        if (!config.IsValid)
            throw new InvalidOperationException($"RPDO {pdoNumber} is not valid");
        
        if (data.Length > 8)
            throw new ArgumentException("PDO data cannot exceed 8 bytes");
        
        // Check xem PDO đã được configure trên device chưa
        if (!_configuredRpdoNumbers.Contains(pdoNumber))
        {
            _logger?.LogWarning("RPDO{PDONumber} is not configured on device Node {NodeId}. Cannot send via PDO.", 
                pdoNumber, _nodeId);
            throw new InvalidOperationException(
                $"RPDO {pdoNumber} is not configured on device Node {_nodeId}. " +
                $"Please ensure PDO configuration is completed before using PDO communication.");
        }
        
        uint cobId = config.CobId & 0x1FFFFFFF; // Mask out valid/RTR bits
        await _canBus.SendFrameAsync(cobId, data, ct);
    }
    
    /// <summary>
    /// Request TPDO từ device (RTR - Remote Transmission Request)
    /// </summary>
    public async Task RequestTPDOAsync(byte pdoNumber, CancellationToken ct = default)
    {
        if (_disposed)
            throw new ObjectDisposedException(nameof(PdoManager));
        
        if (!_tpdoConfigs.TryGetValue(pdoNumber, out var config))
            throw new InvalidOperationException($"TPDO {pdoNumber} not configured");

        if (!config.RtrAllowed)
            throw new InvalidOperationException($"TPDO {pdoNumber} does not allow RTR");

        uint cobId = config.CobId & 0x1FFFFFFF;
        // Send RTR frame (empty data with RTR flag)
        await _canBus.SendFrameAsync(cobId | 0x40000000, [], ct);
    }
    
    /// <summary>
    /// Ghi PDO configuration xuống device qua SDO
    /// </summary>
    /// <param name="device">ICanOpenDevice instance để ghi config</param>
    /// <param name="isTpdo">True nếu là TPDO, False nếu là RPDO</param>
    /// <param name="pdoNumber">PDO number (1-4)</param>
    /// <param name="ct">Cancellation token</param>
    public async Task WritePdoConfigurationToDeviceAsync(ICanOpenDevice device, bool isTpdo, byte pdoNumber, CancellationToken ct = default)
    {
        if (pdoNumber < 1 || pdoNumber > 4)
            throw new ArgumentException("PDO number must be between 1 and 4", nameof(pdoNumber));

        PdoConfiguration? config;
        if (isTpdo)
        {
            if (!_tpdoConfigs.TryGetValue(pdoNumber, out config))
                throw new InvalidOperationException($"TPDO {pdoNumber} not configured");
        }
        else
        {
            if (!_rpdoConfigs.TryGetValue(pdoNumber, out config))
                throw new InvalidOperationException($"RPDO {pdoNumber} not configured");
        }
        
        if (config == null)
            return;
        
        // Validate configuration
        var validation = config.ValidateConfiguration();
        if (!validation.IsValid)
        {
            throw new InvalidOperationException($"PDO {pdoNumber} configuration is invalid: {string.Join(", ", validation.Errors)}");
        }
        
        // Calculate indices theo CANOpen DS301
        // Communication Parameter Index: chứa COB-ID, Transmission Type, Inhibit Time, Event Timer
        // Mapping Parameter Index: chứa mapping count và các mappings
        ushort commParamIndex = isTpdo 
            ? (ushort)(0x1800 + (pdoNumber - 1))  // TPDO1: 0x1800, TPDO2: 0x1801, TPDO3: 0x1802, TPDO4: 0x1803
            : (ushort)(0x1400 + (pdoNumber - 1)); // RPDO1: 0x1400, RPDO2: 0x1401, RPDO3: 0x1402, RPDO4: 0x1403
        
        ushort mappingParamIndex = isTpdo 
            ? (ushort)(0x1A00 + (pdoNumber - 1))  // TPDO1: 0x1A00, TPDO2: 0x1A01, TPDO3: 0x1A02, TPDO4: 0x1A03 (theo CANOpen standard và EDS file)
            : (ushort)(0x1600 + (pdoNumber - 1));         // RPDO1: 0x1600, RPDO2: 0x1601, RPDO3: 0x1602, RPDO4: 0x1603
        
        // Device phải ở PreOperational state để configure PDOs
        // Note: User cần đảm bảo device ở PreOperational trước khi gọi method này
        
        // 1. Disable PDO trước (set bit 31 của COB-ID trong Communication Parameter)
        uint disabledCobId = config.CobId | 0x80000000;
        await device.WriteUInt32Async(commParamIndex, 0x01, disabledCobId, ct);
        await Task.Delay(10, ct); // Small delay để device process
        
        // 2. Clear existing mappings (write 0 to mapping count trong Mapping Parameter)
        await device.WriteUInt8Async(mappingParamIndex, 0x00, 0, ct);
        await Task.Delay(10, ct);
        
        // 3. Write mappings vào Mapping Parameter (sub-index 1, 2, 3, ...)
        byte mappingSubIndex = 1;
        foreach (var mapping in config.Mappings)
        {
            uint mappingValue = mapping.ToMappingValue();
            await device.WriteUInt32Async(mappingParamIndex, mappingSubIndex, mappingValue, ct);
            mappingSubIndex++;
        }
        
        // 4. Write mapping count vào Mapping Parameter (sub-index 0)
        await device.WriteUInt8Async(mappingParamIndex, 0x00, (byte)config.Mappings.Count, ct);
        
        // 5. Write Transmission Type vào Communication Parameter (sub-index 2)
        await device.WriteUInt8Async(commParamIndex, 0x02, (byte)config.TransmissionType, ct);
        
        // 6. Write Inhibit Time (chỉ cho TPDO) vào Communication Parameter (sub-index 3)
        // Lưu ý: InhibitTime ngăn TPDO gửi quá thường xuyên (minimum time between transmissions)
        // Nếu InhibitTime = 0, device có thể dùng default value từ EDS file
        // Để đảm bảo TPDO gửi khi có thay đổi, nên set InhibitTime = 0 hoặc giá trị nhỏ
        if (isTpdo)
        {
            // Nếu config không có InhibitTime, set = 0 để disable inhibit time
            // Điều này cho phép TPDO gửi ngay khi có thay đổi (nếu không có EventTimer)
            ushort inhibitTime = config.InhibitTime;
            await device.WriteUInt16Async(commParamIndex, 0x03, inhibitTime, ct);
        }
        
        // 7. Write Event Timer vào Communication Parameter
        // TPDO: sub-index 0x05 (theo DS301 và EDS file)
        // RPDO: thường không có Event Timer, nhưng nếu có thì ở sub-index 0x03 (theo DS301)
        // Tuy nhiên, nhiều device RPDO không hỗ trợ Event Timer, nên chỉ ghi cho TPDO
        // QUAN TRỌNG: Luôn write EventTimer (kể cả = 0) để clear giá trị cũ trên device
        // Nếu không write, device có thể giữ EventTimer cũ từ lần configure trước
        if (isTpdo)
        {
            try
            {
                await device.WriteUInt16Async(commParamIndex, 0x05, config.EventTimer, ct);
            }
            catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
                                         ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
            {
                // Sub-index 0x05 không tồn tại (device không hỗ trợ EventTimer)
                // Đây không phải lỗi nghiêm trọng, chỉ log warning
                _logger?.LogWarning("TPDO{PDONumber} EventTimer sub-index 0x05 does not exist on device. EventTimer will not be set.", pdoNumber);
            }
        }
        
        // 8. Enable PDO (write COB-ID với bit 31 = 0 vào Communication Parameter, sub-index 0x01)
        // QUAN TRỌNG: Phải enable PDO sau khi đã configure tất cả parameters
        uint enabledCobId = config.CobId & 0x7FFFFFFF; // Clear bit 31
        await device.WriteUInt32Async(commParamIndex, 0x01, enabledCobId, ct);
        await Task.Delay(10, ct); // Small delay để device process
    }
    
    /// <summary>
    /// Verify object tồn tại trên device bằng cách đọc thử
    /// Theo CANOpen standard, nên đọc sub-index 0x00 (highest sub-index supported) để verify object tồn tại
    /// Sub-index 0x00 luôn tồn tại và read-only nếu object tồn tại
    /// </summary>
    private async Task<bool> VerifyObjectExistsAsync(ICanOpenDevice device, ushort index, byte subIndex, CancellationToken ct)
    {
        try
        {
            // Đọc thử object với timeout ngắn
            // Nếu đọc được → object tồn tại
            await device.ReadUInt8Async(index, subIndex, ct);
            return true;
        }
        catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
                                       ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
        {
            // Object hoặc sub-index không tồn tại
            return false;
        }
        catch
        {
            // Các lỗi khác (timeout, etc.) - giả định object không tồn tại hoặc không accessible
            // Lưu ý: Có thể device đang ở trạng thái không cho phép truy cập object dictionary
            return false;
        }
    }
    
    /// <summary>
    /// Clear PDO mapping trên device (nếu object tồn tại)
    /// Dùng để clear mapping cũ trước khi verify và configure mapping mới
    /// </summary>
    private async Task<bool> TryClearPdoMappingAsync(ICanOpenDevice device, ushort mappingParamIndex, CancellationToken ct)
    {
        try
        {
            // Thử clear mapping bằng cách write 0 vào mapping count (sub-index 0)
            // Nếu object không tồn tại, sẽ throw exception
            await device.WriteUInt8Async(mappingParamIndex, 0x00, 0, ct);
            await Task.Delay(10, ct); // Small delay để device process
            return true;
        }
        catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
                                       ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
        {
            // Object không tồn tại
            return false;
        }
        catch
        {
            // Các lỗi khác - giả định object không tồn tại hoặc không accessible
            return false;
        }
    }
    
    /// <summary>
    /// Parse EDS file để lấy danh sách các PDO objects được hỗ trợ
    /// Chỉ match main index entries (không có sub-index)
    /// </summary>
    /// <param name="edsFilePath">Đường dẫn đến EDS file (có thể null)</param>
    /// <returns>HashSet chứa các PDO object indices được hỗ trợ (0x1A00, 0x1A01, 0x1A02, 0x1A03 cho TPDO; 0x1600, 0x1601, 0x1602, 0x1603 cho RPDO)</returns>
    private static HashSet<ushort> ParseSupportedPdoIndicesFromEds(string? edsFilePath)
    {
        var supportedIndices = new HashSet<ushort>();
        
        if (string.IsNullOrWhiteSpace(edsFilePath) || !File.Exists(edsFilePath))
        {
            return supportedIndices; // Empty set
        }
        
        try
        {
            var content = File.ReadAllText(edsFilePath);
            
            // Pattern để tìm các main index entries (không có sub-index):
            // Match [Index] nhưng không match [Index]subX
            // Pattern: [Index] không theo sau bởi "sub"
            var indexPattern = @"\[([0-9A-Fa-f]+)\](?!sub)";
            var matches = Regex.Matches(content, indexPattern, RegexOptions.IgnoreCase);
            
            foreach (Match match in matches)
            {
                if (match.Success && match.Groups.Count > 1)
                {
                    var indexStr = match.Groups[1].Value;
                    if (ushort.TryParse(indexStr, System.Globalization.NumberStyles.HexNumber, null, out ushort index))
                    {
                        // Check if this is a PDO-related index
                        // TPDO Mapping Parameters: 0x1A00, 0x1A01, 0x1A02, 0x1A03 (theo CANOpen standard và EDS file)
                        if (index >= 0x1A00 && index <= 0x1A03)
                        {
                            supportedIndices.Add(index);
                        }
                        // RPDO Mapping Parameters: 0x1600, 0x1601, 0x1602, 0x1603
                        else if (index >= 0x1600 && index <= 0x1603)
                        {
                            supportedIndices.Add(index);
                        }
                        // TPDO Communication Parameters: 0x1800, 0x1801, 0x1802, 0x1803
                        else if (index >= 0x1800 && index <= 0x1803)
                        {
                            supportedIndices.Add(index);
                        }
                        // RPDO Communication Parameters: 0x1400, 0x1401, 0x1402, 0x1403
                        else if (index >= 0x1400 && index <= 0x1403)
                        {
                            supportedIndices.Add(index);
                        }
                    }
                }
            }
        }
        catch
        {
            // Nếu parse EDS fail, return empty set
        }
        
        return supportedIndices;
    }
    
    /// <summary>
    /// Check xem PDO có được hỗ trợ trong EDS không
    /// </summary>
    private static bool IsPdoSupportedInEds(ushort mappingParamIndex, HashSet<ushort> supportedIndices)
    {
        return supportedIndices.Contains(mappingParamIndex);
    }
    
    /// <summary>
    /// Stop và clear tất cả TPDOs trên device
    /// </summary>
    private async Task StopAndClearAllTpdosAsync(ICanOpenDevice device, CancellationToken ct)
    {
        // Loop qua tất cả TPDOs có thể có (1-4)
        for (byte pdoNumber = 1; pdoNumber <= 4; pdoNumber++)
        {
            ushort commParamIndex = (ushort)(0x1800 + (pdoNumber - 1));
            // Theo CANOpen standard và EDS file: TPDO1=0x1A00, TPDO2=0x1A01, TPDO3=0x1A02, TPDO4=0x1A03
            ushort mappingParamIndex = (ushort)(0x1A00 + (pdoNumber - 1));
            
            try
            {
                // 1. Stop PDO: Disable bằng cách set bit 31 của COB-ID
                // Đọc COB-ID hiện tại trước
                try
                {
                    uint currentCobId = await device.ReadUInt32Async(commParamIndex, 0x01, ct);
                    uint disabledCobId = currentCobId | 0x80000000; // Set bit 31 để disable
                    await device.WriteUInt32Async(commParamIndex, 0x01, disabledCobId, ct);
                    await Task.Delay(10, ct);
                }
                catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
                                               ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
                {
                    // Object không tồn tại, skip
                    continue;
                }
                
                // 2. Clear mapping: Write 0 vào mapping count
                try
                {
                    await device.WriteUInt8Async(mappingParamIndex, 0x00, 0, ct);
                    await Task.Delay(10, ct);
                }
                catch (SdoException ex) when (ex.AbortCode == (uint)SdoAbortCode.ObjectDoesNotExist || 
                                               ex.AbortCode == (uint)SdoAbortCode.SubIndexDoesNotExist)
                {
                    // Object không tồn tại, skip
                    continue;
                }
            }
            catch (Exception ex)
            {
                // Log warning nhưng tiếp tục với các PDO khác
                _logger?.LogWarning(ex, "Failed to stop/clear TPDO{PDONumber} on device Node {NodeId}. Continuing...", 
                    pdoNumber, device.NodeId);
            }
        }
    }
    
    /// <summary>
    /// Ghi tất cả TPDO configurations xuống device
    /// Verify object tồn tại trước khi configure
    /// Nếu EDS có nhưng device không hỗ trợ → throw exception
    /// </summary>
    /// <param name="device">CANOpen device</param>
    /// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support)</param>
    /// <param name="ct">Cancellation token</param>
    public async Task WriteAllTpdoConfigurationsToDeviceAsync(ICanOpenDevice device, string? edsFilePath = null, CancellationToken ct = default)
    {
        // 1. Reset CANOpen communication để đảm bảo device ở trạng thái sạch
        try
        {
            await device.ResetCommunicationAsync(ct);
            await Task.Delay(100, ct); // Wait for device to reset
        }
        catch (Exception ex)
        {
            _logger?.LogWarning(ex, "Failed to reset communication on device Node {NodeId}. Continuing...", device.NodeId);
        }
        
        // 2. Đảm bảo device ở PreOperational state (cần thiết để configure PDOs)
        try
        {
            var currentState = device.State;
            if (currentState != NmtState.PreOperational && currentState != NmtState.Stopped)
            {
                await device.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
                await Task.Delay(100, ct); // Wait for state change
            }
        }
        catch (Exception ex)
        {
            _logger?.LogWarning(ex, "Failed to set device Node {NodeId} to PreOperational state. Continuing...", device.NodeId);
        }
        
        // 3. Stop và clear tất cả TPDOs trước khi configure mới
        await StopAndClearAllTpdosAsync(device, ct);
        
        // Parse EDS để lấy danh sách PDO được hỗ trợ
        var supportedIndices = ParseSupportedPdoIndicesFromEds(edsFilePath);
        var useEdsFilter = supportedIndices.Count > 0 && !string.IsNullOrWhiteSpace(edsFilePath);
        
        _configuredTpdoNumbers.Clear(); // Reset tracking
        _lastUnconfiguredTpdoWarningTicks.Clear(); // Reset throttle so we log again after reconfig
        
        foreach (var kvp in _tpdoConfigs)
        {
            // Tính mapping parameter index cho TPDO
            // Theo CANOpen standard và EDS file: TPDO1=0x1A00, TPDO2=0x1A01, TPDO3=0x1A02, TPDO4=0x1A03
            ushort mappingParamIndex = (ushort)(0x1A00 + (kvp.Key - 1));
            ushort commParamIndex = (ushort)(0x1800 + (kvp.Key - 1));
            
            // Nếu có EDS và PDO không được hỗ trợ trong EDS, bỏ qua
            if (useEdsFilter && !IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
            {
                _logger?.LogWarning("TPDO{PDONumber} (0x{MappingIndex:X4}) is not found in EDS file. Skipping configuration.", 
                    kvp.Key, mappingParamIndex);
                continue;
            }
            
            // Verify object tồn tại trên device trước khi configure
            // Quan trọng: Check communication parameter trước, vì nếu comm parameter không tồn tại
            // thì mapping parameter cũng không tồn tại (device không hỗ trợ PDO này)
            // Đọc sub-index 0x00 (highest sub-index supported) để verify object tồn tại - đây là cách chuẩn theo CANOpen
            bool commExists = await VerifyObjectExistsAsync(device, commParamIndex, 0x00, ct);
            
            if (!commExists)
            {
                // Communication parameter không tồn tại → device không hỗ trợ PDO này
                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
                {
                    _logger?.LogError("EDS file mismatch: TPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
                        "Communication parameter 0x{CommIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
                        kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
                    throw new ConfigurationException($"TPDO{kvp.Key}", 
                        $"EDS file declares TPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
                        $"Communication parameter 0x{commParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
                }
                
                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
                _logger?.LogWarning("TPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Communication parameter 0x{CommIndex:X4} does not exist. Skipping configuration.", 
                    kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
                continue;
            }
            
            // Communication parameter tồn tại → tiếp tục verify mapping parameter
            // Clear mapping cũ trước (nếu có) để đảm bảo device ở trạng thái sạch
            // Điều này cũng giúp verify object tồn tại: nếu clear thành công thì object tồn tại
            bool mappingCleared = await TryClearPdoMappingAsync(device, mappingParamIndex, ct);
            bool mappingExists = mappingCleared || await VerifyObjectExistsAsync(device, mappingParamIndex, 0x00, ct);
            
            if (!mappingExists)
            {
                // Mapping parameter không tồn tại (nhưng comm parameter tồn tại - trường hợp hiếm)
                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
                {
                    _logger?.LogError("EDS file mismatch: TPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
                        "Mapping parameter 0x{MappingIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
                        kvp.Key, mappingParamIndex, device.NodeId, mappingParamIndex);
                    throw new ConfigurationException($"TPDO{kvp.Key}", 
                        $"EDS file declares TPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
                        $"Mapping parameter 0x{mappingParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
                }
                
                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
                _logger?.LogWarning("TPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Mapping parameter does not exist. Skipping configuration.", 
                    kvp.Key, mappingParamIndex, device.NodeId);
                continue;
            }
            
            // Object tồn tại → configure
            try
            {
                // Log COB-ID trước khi configure
                var config = _tpdoConfigs[kvp.Key];
                uint cobId = config.CobId & 0x1FFFFFFF;
                
                await WritePdoConfigurationToDeviceAsync(device, true, kvp.Key, ct);
                _configuredTpdoNumbers.Add(kvp.Key);
            }
            catch (Exception ex)
            {
                // Nếu có EDS và EDS nói device hỗ trợ nhưng configure fail → ERROR
                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
                {
                    _logger?.LogError(ex, "Failed to configure TPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId} even though it is declared in EDS file.", 
                        kvp.Key, mappingParamIndex, device.NodeId);
                    throw new ConfigurationException($"TPDO{kvp.Key}", 
                        $"Failed to configure TPDO{kvp.Key} (0x{mappingParamIndex:X4}) on device Node {device.NodeId}: {ex.Message}");
                }
                
                // Nếu không có EDS hoặc EDS không có → chỉ log warning
                _logger?.LogWarning(ex, "Failed to configure TPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId}. Skipping.", 
                    kvp.Key, mappingParamIndex, device.NodeId);
            }
        }
    }
    
    /// <summary>
    /// Ghi tất cả RPDO configurations xuống device
    /// Verify object tồn tại trước khi configure
    /// Nếu EDS có nhưng device không hỗ trợ → throw exception
    /// </summary>
    /// <param name="device">CANOpen device</param>
    /// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support)</param>
    /// <param name="ct">Cancellation token</param>
    public async Task WriteAllRpdoConfigurationsToDeviceAsync(ICanOpenDevice device, string? edsFilePath = null, CancellationToken ct = default)
    {
        // Parse EDS để lấy danh sách PDO được hỗ trợ
        var supportedIndices = ParseSupportedPdoIndicesFromEds(edsFilePath);
        var useEdsFilter = supportedIndices.Count > 0 && !string.IsNullOrWhiteSpace(edsFilePath);
        
        _configuredRpdoNumbers.Clear(); // Reset tracking
        
        foreach (var kvp in _rpdoConfigs)
        {
            // Tính mapping parameter index cho RPDO
            ushort mappingParamIndex = (ushort)(0x1600 + (kvp.Key - 1));
            ushort commParamIndex = (ushort)(0x1400 + (kvp.Key - 1));
            
            // Nếu có EDS và PDO không được hỗ trợ trong EDS, bỏ qua
            if (useEdsFilter && !IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
            {
                _logger?.LogWarning("RPDO{PDONumber} (0x{MappingIndex:X4}) is not found in EDS file. Skipping configuration.", 
                    kvp.Key, mappingParamIndex);
                continue;
            }
            
            // Verify object tồn tại trên device trước khi configure
            // Quan trọng: Check communication parameter trước, vì nếu comm parameter không tồn tại
            // thì mapping parameter cũng không tồn tại (device không hỗ trợ PDO này)
            // Đọc sub-index 0x00 (highest sub-index supported) để verify object tồn tại - đây là cách chuẩn theo CANOpen
            bool commExists = await VerifyObjectExistsAsync(device, commParamIndex, 0x00, ct);
            
            if (!commExists)
            {
                // Communication parameter không tồn tại → device không hỗ trợ PDO này
                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
                {
                    _logger?.LogError("EDS file mismatch: RPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
                        "Communication parameter 0x{CommIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
                        kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
                    throw new ConfigurationException($"RPDO{kvp.Key}", 
                        $"EDS file declares RPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
                        $"Communication parameter 0x{commParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
                }
                
                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
                _logger?.LogWarning("RPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Communication parameter 0x{CommIndex:X4} does not exist. Skipping configuration.", 
                    kvp.Key, mappingParamIndex, device.NodeId, commParamIndex);
                continue;
            }
            
            // Communication parameter tồn tại → tiếp tục verify mapping parameter
            // Clear mapping cũ trước (nếu có) để đảm bảo device ở trạng thái sạch
            // Điều này cũng giúp verify object tồn tại: nếu clear thành công thì object tồn tại
            bool mappingCleared = await TryClearPdoMappingAsync(device, mappingParamIndex, ct);
            bool mappingExists = mappingCleared || await VerifyObjectExistsAsync(device, mappingParamIndex, 0x00, ct);
            
            if (!mappingExists)
            {
                // Mapping parameter không tồn tại (nhưng comm parameter tồn tại - trường hợp hiếm)
                // Nếu có EDS file và EDS nói device hỗ trợ nhưng device không hỗ trợ → ERROR
                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
                {
                    _logger?.LogError("EDS file mismatch: RPDO{PDONumber} (0x{MappingIndex:X4}) is declared in EDS file but device Node {NodeId} does not support it. " +
                        "Mapping parameter 0x{MappingIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.", 
                        kvp.Key, mappingParamIndex, device.NodeId, mappingParamIndex);
                    throw new ConfigurationException($"RPDO{kvp.Key}", 
                        $"EDS file declares RPDO{kvp.Key} (0x{mappingParamIndex:X4}) is supported, but device Node {device.NodeId} does not support it. " +
                        $"Mapping parameter 0x{mappingParamIndex:X4} does not exist. Please verify EDS file matches the actual device firmware version.");
                }
                
                // Nếu không có EDS hoặc EDS không có → chỉ log warning và skip
                _logger?.LogWarning("RPDO{PDONumber} (0x{MappingIndex:X4}) is not supported by device Node {NodeId}. Mapping parameter does not exist. Skipping configuration.", 
                    kvp.Key, mappingParamIndex, device.NodeId);
                continue;
            }
            
            // Object tồn tại → configure
            try
            {
                await WritePdoConfigurationToDeviceAsync(device, false, kvp.Key, ct);
                _configuredRpdoNumbers.Add(kvp.Key);
            }
            catch (Exception ex)
            {
                // Nếu có EDS và EDS nói device hỗ trợ nhưng configure fail → ERROR
                if (useEdsFilter && IsPdoSupportedInEds(mappingParamIndex, supportedIndices))
                {
                    _logger?.LogError(ex, "Failed to configure RPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId} even though it is declared in EDS file.", 
                        kvp.Key, mappingParamIndex, device.NodeId);
                    throw new ConfigurationException($"RPDO{kvp.Key}", 
                        $"Failed to configure RPDO{kvp.Key} (0x{mappingParamIndex:X4}) on device Node {device.NodeId}: {ex.Message}");
                }
                
                // Nếu không có EDS hoặc EDS không có → chỉ log warning
                _logger?.LogWarning(ex, "Failed to configure RPDO{PDONumber} (0x{MappingIndex:X4}) on device Node {NodeId}. Skipping.", 
                    kvp.Key, mappingParamIndex, device.NodeId);
            }
        }
    }
    
    /// <summary>
    /// Ghi tất cả PDO configurations (TPDO + RPDO) xuống device
    /// Chỉ configure các PDO được hỗ trợ trong EDS file (nếu có)
    /// </summary>
    /// <param name="device">CANOpen device</param>
    /// <param name="edsFilePath">Đường dẫn đến EDS file (optional, để check PDO support)</param>
    /// <param name="ct">Cancellation token</param>
    public async Task WriteAllPdoConfigurationsToDeviceAsync(ICanOpenDevice device, string? edsFilePath = null, CancellationToken ct = default)
    {
        await WriteAllTpdoConfigurationsToDeviceAsync(device, edsFilePath, ct);
        await WriteAllRpdoConfigurationsToDeviceAsync(device, edsFilePath, ct);
    }
    
    private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
    {
        // Create snapshot of configs to avoid issues during iteration
        // ConcurrentDictionary.Values is thread-safe for iteration, but snapshot is safer
        var configs = _tpdoConfigs.Values.ToArray();
        
        // Check if this is a TPDO for this node
        foreach (var config in configs)
        {
            uint expectedCobId = config.CobId & 0x1FFFFFFF;
            
            // Debug logging để kiểm tra matching
            if (e.CanId == expectedCobId)
            {
                if (!config.IsValid)
                {
                    _logger?.LogWarning("TPDO{PDONumber} frame received (COB-ID=0x{CobId:X3}) but config is invalid (disabled). Expected COB-ID=0x{ExpectedCobId:X3}", 
                        config.PdoNumber, e.CanId, expectedCobId);
                    continue;
                }
                
                // Check if TPDO was configured on device
                if (!_configuredTpdoNumbers.Contains(config.PdoNumber))
                {
                    // Throttle repeated warnings to avoid log spam
                    var now = Environment.TickCount64;
                    var lastLog = _lastUnconfiguredTpdoWarningTicks.GetOrAdd(config.PdoNumber, 0L);
                    if (now - lastLog >= UnconfiguredTpdoWarningIntervalMs)
                    {
                        _lastUnconfiguredTpdoWarningTicks[config.PdoNumber] = now;
                        _logger?.LogWarning("TPDO{PDONumber} frame received (COB-ID=0x{CobId:X3}) but TPDO{PDONumber} was not successfully configured on device Node {NodeId}. Skipping. (This warning is throttled to once per minute.)", 
                            config.PdoNumber, e.CanId, config.PdoNumber, _nodeId);
                    }
                    continue;
                }
                
                // Match found - invoke event
                var pdoData = new PdoData(config.PdoNumber, e.CanId, e.Data, e.Timestamp);
                PdoReceived?.Invoke(this, new PdoReceivedEventArgs(pdoData, PdoType.Transmit));
                return;
            }
        }
        
        // Log unmatched frames that might be TPDOs (check if COB-ID is in TPDO range)
        // TPDO COB-ID range: 0x180-0x1FF, 0x280-0x2FF, 0x380-0x3FF, 0x480-0x4FF (TPDO1-4 for nodes 1-127)
        if ((e.CanId >= 0x180 && e.CanId <= 0x1FF) || 
            (e.CanId >= 0x280 && e.CanId <= 0x2FF) || 
            (e.CanId >= 0x380 && e.CanId <= 0x3FF) || 
            (e.CanId >= 0x480 && e.CanId <= 0x4FF))
        {
            _logger?.LogTrace("Received CAN frame with COB-ID=0x{CobId:X3} (possible TPDO) but no matching TPDO config found. Configured TPDOs: {TpdoList}, Configured on device: {ConfiguredList}", 
                e.CanId, 
                string.Join(", ", configs.Select(c => $"TPDO{c.PdoNumber}(0x{c.CobId & 0x1FFFFFFF:X3})")),
                string.Join(", ", _configuredTpdoNumbers.Select(n => $"TPDO{n}")));
        }
    }
    
    public void Dispose()
    {
        if (_disposed)
            return;
        
        _canBus.FrameReceived -= OnFrameReceived;
        _tpdoConfigs.Clear();
        _rpdoConfigs.Clear();
        _configuredTpdoNumbers.Clear();
        _configuredRpdoNumbers.Clear();
        _lastUnconfiguredTpdoWarningTicks.Clear();
        
        _disposed = true;
        GC.SuppressFinalize(this);
    }
}

/// <summary>
/// Event args cho PDO received
/// </summary>
public class PdoReceivedEventArgs(PdoData data, PdoType type) : EventArgs
{
    public PdoData Data { get; } = data;
    public PdoType Type { get; } = type;
}