using RobotNet10.CANOpen;
using RobotNet10.CANOpen.CiA402;
using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.CANOpen.CiA402.Models;
using RobotNet10.CANOpen.Enums;
using RobotNet10.CANOpen.Exceptions;
using RobotNet10.CANOpen.Models;
using RobotNet10.CANOpen.Services;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using System.Collections.Concurrent;

namespace RobotNet10.RobotApp.Drivers.PhenikaaX;

/// <summary>
/// Cấu hình đầy đủ cho MBDV_DulAxes driver
/// </summary>
internal class MbdvDulAxesConfig
{
    public string CanInterface { get; set; } = string.Empty;
    public string NodeId { get; set; } = string.Empty;
    public bool? AutoReconnectEnabled { get; set; }
    public int? ReconnectDelayMs { get; set; }
    public int? MaxReconnectAttempts { get; set; }
    public int? PdoConfigRetryCount { get; set; }
    public int? PdoConfigRetryTimeoutMs { get; set; }
    public bool? UseHeartbeatCheck { get; set; }
    public PdoMappingsConfig? PdoMappings { get; set; }
    /// <summary>
    /// Danh sách các homing method được phép sử dụng
    /// Nếu null hoặc empty, cho phép tất cả methods
    /// </summary>
    public List<byte>? AllowedHomingMethods { get; set; }
}

/// <summary>
/// Cấu hình PDO Mappings
/// </summary>
internal class PdoMappingsConfig
{
    public List<PdoMappingConfig>? RPDO1 { get; set; }
    public List<PdoMappingConfig>? RPDO2 { get; set; }
    public List<PdoMappingConfig>? RPDO3 { get; set; }
    public List<PdoMappingConfig>? RPDO4 { get; set; }
    public List<PdoMappingConfig>? TPDO1 { get; set; }
    public List<PdoMappingConfig>? TPDO2 { get; set; }
    public List<PdoMappingConfig>? TPDO3 { get; set; }
    public List<PdoMappingConfig>? TPDO4 { get; set; }
}

/// <summary>
/// PDO Mapping Configuration từ config file
/// </summary>
internal class PdoMappingConfig
{
    public string Index { get; set; } = string.Empty;
    public byte SubIndex { get; set; }
    public byte BitLength { get; set; }
}

/// <summary>
/// Thông tin mapping của một object vào PDO
/// </summary>
internal class PdoMappingInfo
{
    public bool IsRPDO { get; set; }
    public byte PdoNumber { get; set; }
    public int BitOffset { get; set; }
    public byte BitLength { get; set; }
    public ushort ObjectIndex { get; set; }
    public byte SubIndex { get; set; }
}

/// <summary>
/// Driver cho Phenikaa X servo motor theo chuẩn CiA402
/// Kế thừa DeviceBase và implement ICiA402Servo
/// Sử dụng tối đa TPDO và RPDO dựa trên EDS file
/// </summary>
[Device(DeviceType.CiA402Servo, "PhenikaaX", "CiA402Servo", "1.0.0", Description = "Phenikaa X CiA402 Servo Driver")]
public class CiA402Servo : DeviceBase, ICiA402Servo
{
    private readonly ICanOpenManager? _canOpenManager;
    private readonly string _canInterface;
    private readonly byte _nodeId;
    private readonly int _pdoConfigRetryCount;
    private readonly int _pdoConfigRetryTimeoutMs;
    private readonly bool _useHeartbeatCheck;
    private readonly ILogger _logger;
    private CanOpenDevice? _canOpenDevice;
    private readonly CiA402ObjectDictionary _objectDictionary = CiA402ObjectDictionary.CreateDefault();
    
    // PDO Mapping Configuration
    private readonly Dictionary<string, List<PdoMappingConfig>> _pdoMappingsConfig = [];
    
    // Mapping registry: object index -> PDO mapping info
    // Key: (ushort index, byte subIndex), Value: PdoMappingInfo
    private readonly ConcurrentDictionary<(ushort Index, byte SubIndex), PdoMappingInfo> _pdoMappingRegistry = new();
    
    // Allowed homing methods (null hoặc empty = cho phép tất cả)
    private readonly HashSet<byte>? _allowedHomingMethods;
    
    // Thread-safe cached values
    private readonly Lock _lock = new();
    private Statusword _cachedStatusword;
    private int _cachedPosition;
    private int _cachedVelocity;
    private short _cachedTorque;
    private int _targetPosition;
    
    // Cache for all mapped objects (key: object index, value: cached value as byte array)
    private readonly ConcurrentDictionary<ushort, byte[]> _objectCache = new();

    // Events
    public event EventHandler<StatuswordChangedEventArgs>? StatuswordChanged;
    public event EventHandler<PositionChangedEventArgs>? PositionChanged;
    public event EventHandler<VelocityChangedEventArgs>? VelocityChanged;

    /// <summary>
    /// Constructor với IServiceProvider để lấy ICanOpenManager
    /// </summary>
    public CiA402Servo(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
        : base(deviceId, deviceName, DeviceType.CiA402Servo)
    {
        // Bind tất cả cấu hình từ IConfigurationSection
        var config = new MbdvDulAxesConfig();
        connection.Bind(config);

        // Validate required fields
        if (string.IsNullOrWhiteSpace(config.CanInterface))
            throw new InvalidOperationException("CanInterface is required in connection configuration");
        
        if (string.IsNullOrWhiteSpace(config.NodeId))
            throw new InvalidOperationException("NodeId is required in connection configuration");

        _canInterface = config.CanInterface;
        
        if (!byte.TryParse(config.NodeId, out _nodeId) || _nodeId == 0 || _nodeId > 127)
        {
            throw new InvalidOperationException($"Invalid NodeId: {config.NodeId}. Must be between 1 and 127");
        }

        // PDO Configuration retry settings
        _pdoConfigRetryCount = config.PdoConfigRetryCount ?? 3;
        _pdoConfigRetryTimeoutMs = config.PdoConfigRetryTimeoutMs ?? 1000;
        
        // Heartbeat check option (default: true)
        _useHeartbeatCheck = config.UseHeartbeatCheck ?? true;

        // Cấu hình device
        AutoReconnectEnabled = config.AutoReconnectEnabled ?? true;
        ReconnectDelayMs = config.ReconnectDelayMs ?? 3000;
        MaxReconnectAttempts = config.MaxReconnectAttempts ?? 0;

        // Allowed homing methods
        if (config.AllowedHomingMethods != null && config.AllowedHomingMethods.Count > 0)
        {
            _allowedHomingMethods = [.. config.AllowedHomingMethods];
        }

        // Khởi tạo giá trị properties
        SetProperty("CanInterface", _canInterface);
        SetProperty("NodeId", _nodeId.ToString());
        SetProperty("UseHeartbeatCheck", _useHeartbeatCheck.ToString());

        _canOpenManager = serviceProvider.GetRequiredService<ICanOpenManager>();
        _logger = serviceProvider.GetRequiredService<ILoggerFactory>().CreateLogger<CiA402Servo>();;
        
        _cachedStatusword = new Statusword(0);
        
        // Parse PDO mapping configuration từ config object
        ParsePdoMappingConfiguration(config.PdoMappings);
    }
    
    /// <summary>
    /// Parse PDO mapping configuration từ PdoMappingsConfig object
    /// </summary>
    private void ParsePdoMappingConfiguration(PdoMappingsConfig? pdoMappings)
    {
        if (pdoMappings == null)
        {
            // Nếu không có config, sử dụng default mappings
            return;
        }
        
        // Parse RPDO1-4
        if (pdoMappings.RPDO1 != null && pdoMappings.RPDO1.Count > 0)
        {
            _pdoMappingsConfig["RPDO1"] = pdoMappings.RPDO1;
        }
        
        if (pdoMappings.RPDO2 != null && pdoMappings.RPDO2.Count > 0)
        {
            _pdoMappingsConfig["RPDO2"] = pdoMappings.RPDO2;
        }
        
        if (pdoMappings.RPDO3 != null && pdoMappings.RPDO3.Count > 0)
        {
            _pdoMappingsConfig["RPDO3"] = pdoMappings.RPDO3;
        }
        
        if (pdoMappings.RPDO4 != null && pdoMappings.RPDO4.Count > 0)
        {
            _pdoMappingsConfig["RPDO4"] = pdoMappings.RPDO4;
        }
        
        // Parse TPDO1-4
        if (pdoMappings.TPDO1 != null && pdoMappings.TPDO1.Count > 0)
        {
            _pdoMappingsConfig["TPDO1"] = pdoMappings.TPDO1;
        }
        
        if (pdoMappings.TPDO2 != null && pdoMappings.TPDO2.Count > 0)
        {
            _pdoMappingsConfig["TPDO2"] = pdoMappings.TPDO2;
        }
        
        if (pdoMappings.TPDO3 != null && pdoMappings.TPDO3.Count > 0)
        {
            _pdoMappingsConfig["TPDO3"] = pdoMappings.TPDO3;
        }
        
        if (pdoMappings.TPDO4 != null && pdoMappings.TPDO4.Count > 0)
        {
            _pdoMappingsConfig["TPDO4"] = pdoMappings.TPDO4;
        }
    }

    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
    {
        yield return new PropertyDescription("CanInterface", "CAN Interface", "Tên CAN interface (ví dụ: can0, can1)")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 1,
            Category = "Kết nối",
            DefaultValue = ""
        };

        yield return new PropertyDescription("NodeId", "Node ID", "CANOpen Node ID (1-127)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 2,
            Category = "Kết nối",
            DefaultValue = "1"
        };

        yield return new PropertyDescription("UseHeartbeatCheck", "Use Heartbeat Check", "Bật/tắt kiểm tra heartbeat")
        {
            DataType = "boolean",
            IsReadOnly = true,
            DisplayOrder = 4,
            Category = "Cấu hình",
            DefaultValue = "true"
        };

        yield return new PropertyDescription("Statusword", "Statusword", "Trạng thái servo (hex)")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 5,
            Category = "Trạng thái",
            DefaultValue = "0x0000"
        };

        yield return new PropertyDescription("Position", "Vị trí", "Vị trí hiện tại (encoder counts)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 6,
            Category = "Trạng thái",
            DefaultValue = "0"
        };

        yield return new PropertyDescription("Velocity", "Vận tốc", "Vận tốc hiện tại (encoder counts/s)")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 7,
            Category = "Trạng thái",
            DefaultValue = "0"
        };

        yield return new PropertyDescription("Torque", "Torque", "Torque hiện tại")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 8,
            Category = "Trạng thái",
            DefaultValue = "0"
        };

        yield return new PropertyDescription("DriveState", "Drive State", "Trạng thái drive")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 9,
            Category = "Trạng thái",
            DefaultValue = "Unknown"
        };
    }

    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        if (_canOpenManager == null)
        {
            throw new InvalidOperationException("ICanOpenManager service is required. Make sure to register it in DI container.");
        }
        // Lấy hoặc tạo CanOpenDevice
        _canOpenDevice = await _canOpenManager.GetOrCreateDeviceAsync(_canInterface, _nodeId, cancellationToken);
        
        // Subscribe to PDO events
        _canOpenDevice.PdoReceived += OnPdoReceived;
        
        // Configure PDOs (master-side)
        ConfigurePdos();
        UpdateProperties();
    }

    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized. Call InitializeAsync first.");

        // Kiểm tra kết nối ICanBus
        if (_canOpenManager != null)
        {
            var canBus = _canOpenManager.GetCanBus(_canInterface);
            if (canBus == null || !canBus.IsConnected)
            {
                throw new InvalidOperationException($"ICanBus for interface '{_canInterface}' is not connected.");
            }
        }

        // Enable heartbeat monitoring
        if (_useHeartbeatCheck)
        {
            _canOpenDevice.AutoVerifyStateViaHeartbeat = true;
            _canOpenDevice.EnableHeartbeatMonitoring(timeoutMs: 2000);
        }

        // Configure PDOs trên device (ghi xuống device qua SDO)
        await ConfigureDevicePdosAsync(cancellationToken);

        // Đảm bảo device ở PreOperational state trước khi start node
        var currentState = _canOpenDevice.State;
        if (currentState != NmtState.PreOperational && currentState != NmtState.Stopped)
        {
            await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, cancellationToken);
            await Task.Delay(100, cancellationToken);
        }

        // Start node (chuyển sang Operational state)
        await _canOpenDevice.StartNodeAsync(cancellationToken);

        UpdateProperties();
    }

    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        if (_canOpenDevice != null)
        {
            await _canOpenDevice.StopNodeAsync(cancellationToken);
            _canOpenDevice.DisableHeartbeatMonitoring();
        }
    }

    protected override async Task OnResetAsync(CancellationToken cancellationToken)
    {
        // Reset về trạng thái ban đầu
        try
        {
            await DisableAsync(cancellationToken);
            await Task.Delay(100, cancellationToken);
            
            var state = await GetStateAsync(cancellationToken);
            if (state == DriveState.Fault)
            {
                await FaultResetAsync(cancellationToken);
                await Task.Delay(100, cancellationToken);
            }
            
            await ShutdownAsync(cancellationToken);
            await Task.Delay(100, cancellationToken);
        }
        catch (Exception ex)
        {
            // Log errors during reset nhưng không throw để không block reset process
            _logger.LogWarning(ex, "Error occurred during reset for Node {NodeId}", _nodeId);
        }

        UpdateProperties();
    }

    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
    {
        if (_canOpenDevice == null)
            return false;

        try
        {
            // Check heartbeat
            if (_useHeartbeatCheck)
            {
                var nodeInfo = _canOpenDevice.Heartbeat.GetNodeInfo(_nodeId);
                if (nodeInfo == null || !nodeInfo.IsAlive)
                {
                    return false;
                }
            }

            // Try to read statusword
            await GetStatuswordAsync(cancellationToken);
            return true;
        }
        catch(Exception ex)
        {
            _logger.LogError(ex, "Error checking connection for Node {NodeId}", _nodeId);
            return false;
        }
    }

    /// <summary>
    /// Configure PDOs trên device (ghi xuống device qua SDO)
    /// Sử dụng tối đa TPDO và RPDO dựa trên EDS file
    /// </summary>
    private async Task ConfigureDevicePdosAsync(CancellationToken ct)
    {
        // Retry logic với state verification
        Exception? lastException = null;
        for (int attempt = 1; attempt <= _pdoConfigRetryCount; attempt++)
        {
            try
            {
                // Verify và đảm bảo device ở PreOperational state
                await EnsurePreOperationalStateAsync(ct);
                
                // Ghi tất cả PDO configurations xuống device
                await _canOpenDevice!.WriteAllPdoConfigurationsToDeviceAsync(null, ct);
                
                return;
            }
            catch (CanOpenTimeoutException timeoutEx)
            {
                lastException = timeoutEx;
                
                _logger.LogWarning("PDO configuration attempt {Attempt}/{MaxRetries} failed for Node {NodeId}: {Error}", 
                    attempt, _pdoConfigRetryCount, _nodeId, timeoutEx.Message);
                
                if (attempt < _pdoConfigRetryCount)
                {
                    int delayMs = _pdoConfigRetryTimeoutMs * attempt;
                    await Task.Delay(delayMs, ct);
                    
                    try
                    {
                        await _canOpenDevice!.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
                        await Task.Delay(200, ct);
                    }
                    catch
                    {
                        // Ignore errors
                    }
                }
            }
            catch (Exception)
            {
                throw;
            }
        }
        
        throw new InvalidOperationException(
            $"Failed to configure PDOs after {_pdoConfigRetryCount} attempts for Node {_nodeId}. " +
            $"Last error: {lastException?.Message}", lastException);
    }
    
    private async Task EnsurePreOperationalStateAsync(CancellationToken ct)
    {
        var currentState = _canOpenDevice!.State;
        if (currentState != NmtState.PreOperational)
        {
            await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
            await Task.Delay(100, ct);
        }
        
        if (_canOpenDevice is CanOpenDevice canOpenDevice)
        {
            try
            {
                var verified = await canOpenDevice.VerifyStateViaHeartbeatAsync(timeoutMs: 1000, ct);
                if (verified)
                {
                    var heartbeatState = canOpenDevice.Heartbeat.GetNodeInfo(_nodeId)?.LastState;
                    if (heartbeatState.HasValue && heartbeatState.Value != NmtState.PreOperational)
                    {
                        await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, ct);
                        await Task.Delay(200, ct);
                    }
                }
            }
            catch
            {
                // Ignore
            }
        }
    }
    
    /// <summary>
    /// Configure PDOs cho Phenikaa X - sử dụng configuration từ IConfigurationSection
    /// Nếu không có config, sử dụng default mappings
    /// </summary>
    private void ConfigurePdos()
    {
        if (_objectDictionary == null || _canOpenDevice == null)
            return;

        // Clear mapping registry trước khi configure lại
        _pdoMappingRegistry.Clear();
        _objectCache.Clear();

        // Configure RPDO1-4 từ configuration
        for (byte pdoNum = 1; pdoNum <= 4; pdoNum++)
        {
            var pdoKey = $"RPDO{pdoNum}";
            if (_pdoMappingsConfig.TryGetValue(pdoKey, out var mappings) && mappings.Count > 0)
            {
                ConfigureRPDO(pdoNum, mappings);
            }
            else if (pdoNum == 1)
            {
                // Default RPDO1: Controlword
                var defaultMappings = new List<PdoMappingConfig>
                {
                    new() { Index = "0x6040", SubIndex = 0, BitLength = 16 }
                };
                ConfigureRPDO(pdoNum, defaultMappings);
            }
            else if (pdoNum == 2)
            {
                // Default RPDO2: TargetPosition
                var defaultMappings = new List<PdoMappingConfig>
                {
                    new() { Index = "0x607A", SubIndex = 0, BitLength = 32 }
                };
                ConfigureRPDO(pdoNum, defaultMappings);
            }
            else if (pdoNum == 3)
            {
                // Default RPDO3: TargetVelocity
                var defaultMappings = new List<PdoMappingConfig>
                {
                    new() { Index = "0x60FF", SubIndex = 0, BitLength = 32 }
                };
                ConfigureRPDO(pdoNum, defaultMappings);
            }
            else if (pdoNum == 4)
            {
                // Default RPDO4: TargetTorque
                var defaultMappings = new List<PdoMappingConfig>
                {
                    new() { Index = "0x6071", SubIndex = 0, BitLength = 16 }
                };
                ConfigureRPDO(pdoNum, defaultMappings);
            }
        }

        // Configure TPDO1-4 từ configuration
        for (byte pdoNum = 1; pdoNum <= 4; pdoNum++)
        {
            var pdoKey = $"TPDO{pdoNum}";
            if (_pdoMappingsConfig.TryGetValue(pdoKey, out var mappings) && mappings.Count > 0)
            {
                ConfigureTPDO(pdoNum, mappings);
            }
            else if (pdoNum == 1)
            {
                // Default TPDO1: Statusword + PositionActualValue
                var defaultMappings = new List<PdoMappingConfig>
                {
                    new() { Index = "0x6041", SubIndex = 0, BitLength = 16 },
                    new() { Index = "0x6064", SubIndex = 0, BitLength = 32 }
                };
                ConfigureTPDO(pdoNum, defaultMappings);
            }
            else if (pdoNum == 2)
            {
                // Default TPDO2: VelocityActualValue
                var defaultMappings = new List<PdoMappingConfig>
                {
                    new() { Index = "0x606C", SubIndex = 0, BitLength = 32 }
                };
                ConfigureTPDO(pdoNum, defaultMappings);
            }
        }
    }

    /// <summary>
    /// Configure một RPDO từ danh sách mappings
    /// </summary>
    private void ConfigureRPDO(byte pdoNumber, List<PdoMappingConfig> mappings)
    {
        if (_canOpenDevice == null) return;

        var rpdo = new PdoConfiguration(pdoNumber, (uint)(0x200 + (pdoNumber - 1) * 0x100 + _nodeId));
        int bitOffset = 0;

        foreach (var mappingConfig in mappings)
        {
            if (!CiA402Helper.TryParseObjectIndex(mappingConfig.Index, out ushort objectIndex))
            {
                _logger.LogWarning("Invalid object index in RPDO{Number} mapping: {Index}", pdoNumber, mappingConfig.Index);
                continue;
            }

            // Validate bitLength
            if (mappingConfig.BitLength == 0 || mappingConfig.BitLength > 64)
            {
                _logger.LogWarning("Invalid bitLength ({BitLength}) in RPDO{Number} mapping for object 0x{Index:X4}. Must be between 1 and 64", 
                    mappingConfig.BitLength, pdoNumber, objectIndex);
                continue;
            }

            var mapping = new PdoMapping(objectIndex, mappingConfig.SubIndex, mappingConfig.BitLength);
            rpdo.AddMapping(mapping);

            // Register mapping vào registry
            var key = (objectIndex, mappingConfig.SubIndex);
            _pdoMappingRegistry[key] = new PdoMappingInfo
            {
                IsRPDO = true,
                PdoNumber = pdoNumber,
                BitOffset = bitOffset,
                BitLength = mappingConfig.BitLength,
                ObjectIndex = objectIndex,
                SubIndex = mappingConfig.SubIndex
            };

            bitOffset += mappingConfig.BitLength;
        }

        if (rpdo.Mappings.Count > 0)
        {
            _canOpenDevice.Pdo.ConfigureRPDO(rpdo);
        }
    }

    /// <summary>
    /// Configure một TPDO từ danh sách mappings
    /// </summary>
    private void ConfigureTPDO(byte pdoNumber, List<PdoMappingConfig> mappings)
    {
        if (_canOpenDevice == null) return;

        var tpdo = new PdoConfiguration(pdoNumber, (uint)(0x180 + (pdoNumber - 1) * 0x100 + _nodeId))
        {
            EventTimer = 50 // Default event timer
        };
        int bitOffset = 0;

        foreach (var mappingConfig in mappings)
        {
            if (!CiA402Helper.TryParseObjectIndex(mappingConfig.Index, out ushort objectIndex))
            {
                _logger.LogWarning("Invalid object index in TPDO{Number} mapping: {Index}", pdoNumber, mappingConfig.Index);
                continue;
            }

            // Validate bitLength
            if (mappingConfig.BitLength == 0 || mappingConfig.BitLength > 64)
            {
                _logger.LogWarning("Invalid bitLength ({BitLength}) in TPDO{Number} mapping for object 0x{Index:X4}. Must be between 1 and 64", 
                    mappingConfig.BitLength, pdoNumber, objectIndex);
                continue;
            }

            var mapping = new PdoMapping(objectIndex, mappingConfig.SubIndex, mappingConfig.BitLength);
            tpdo.AddMapping(mapping);

            // Register mapping vào registry
            var key = (objectIndex, mappingConfig.SubIndex);
            _pdoMappingRegistry[key] = new PdoMappingInfo
            {
                IsRPDO = false,
                PdoNumber = pdoNumber,
                BitOffset = bitOffset,
                BitLength = mappingConfig.BitLength,
                ObjectIndex = objectIndex,
                SubIndex = mappingConfig.SubIndex
            };

            bitOffset += mappingConfig.BitLength;
        }

        if (tpdo.Mappings.Count > 0)
        {
            _canOpenDevice.Pdo.ConfigureTPDO(tpdo);
        }
    }


    /// <summary>
    /// Xử lý PDO data nhận được - parse động dựa trên configuration
    /// </summary>
    private void OnPdoReceived(object? sender, PdoReceivedEventArgs e)
    {
        var data = e.Data;
        
        // Tìm tất cả mappings cho TPDO này
        var tpdoMappings = _pdoMappingRegistry.Values
            .Where(m => !m.IsRPDO && m.PdoNumber == data.PdoNumber)
            .OrderBy(m => m.BitOffset)
            .ToList();

        if (tpdoMappings.Count == 0)
        {
            UpdateProperties();
            return;
        }

        // Tính tổng số bytes cần thiết
        int totalBits = tpdoMappings.Sum(m => m.BitLength);
        int totalBytes = (totalBits + 7) / 8; // Round up

        if (data.Data.Length < totalBytes)
        {
            _logger.LogWarning("TPDO{Number} data length ({Length}) is less than expected ({Expected})",
                data.PdoNumber, data.Data.Length, totalBytes);
            UpdateProperties();
            return;
        }

        lock (_lock)
        {
            // Parse từng mapping trong TPDO
            foreach (var mappingInfo in tpdoMappings)
            {
                ProcessTpdoMapping(data.Data, mappingInfo);
            }
        }

        UpdateProperties();
    }

    /// <summary>
    /// Xử lý một mapping trong TPDO data và cập nhật cache
    /// </summary>
    private void ProcessTpdoMapping(byte[] pdoData, PdoMappingInfo mappingInfo)
    {
        try
        {
            // Extract value từ PDO data
            byte[] valueBytes = CiA402Helper.ExtractValueFromPdoData(pdoData, mappingInfo.BitOffset, mappingInfo.BitLength);
            
            // Cache giá trị
            _objectCache[mappingInfo.ObjectIndex] = valueBytes;

            // Xử lý các objects đặc biệt (Statusword, Position, Velocity, Torque)
            switch (mappingInfo.ObjectIndex)
            {
                case 0x6041: // Statusword
                    if (mappingInfo.BitLength == 16 && valueBytes.Length >= 2)
                    {
                        var statuswordValue = BitConverter.ToUInt16(valueBytes, 0);
                        var newStatusword = new Statusword(statuswordValue);
                        if (newStatusword.Value != _cachedStatusword.Value)
                        {
                            var oldState = _cachedStatusword.GetState();
                            var newState = newStatusword.GetState();
                            _cachedStatusword = newStatusword;
                            
                            StatuswordChanged?.Invoke(this, new StatuswordChangedEventArgs(newStatusword, oldState, newState));
                        }
                    }
                    break;

                case 0x6064: // PositionActualValue
                    if (mappingInfo.BitLength == 32 && valueBytes.Length >= 4)
                    {
                        var position = BitConverter.ToInt32(valueBytes, 0);
                        if (position != _cachedPosition)
                        {
                            var oldPosition = _cachedPosition;
                            _cachedPosition = position;
                            PositionChanged?.Invoke(this, new PositionChangedEventArgs(position, oldPosition));
                        }
                    }
                    break;

                case 0x606C: // VelocityActualValue
                    if (mappingInfo.BitLength == 32 && valueBytes.Length >= 4)
                    {
                        var velocity = BitConverter.ToInt32(valueBytes, 0);
                        if (velocity != _cachedVelocity)
                        {
                            var oldVelocity = _cachedVelocity;
                            _cachedVelocity = velocity;
                            VelocityChanged?.Invoke(this, new VelocityChangedEventArgs(velocity, oldVelocity));
                        }
                    }
                    break;

                case 0x6077: // TorqueActualValue
                    if (mappingInfo.BitLength == 16 && valueBytes.Length >= 2)
                    {
                        var torque = (short)BitConverter.ToUInt16(valueBytes, 0);
                        _cachedTorque = torque;
                    }
                    break;

                default:
                    // Các objects khác chỉ cache, không trigger events
                    _logger.LogTrace("Cached object 0x{Index:X4}:{SubIndex} from TPDO{Number}",
                        mappingInfo.ObjectIndex, mappingInfo.SubIndex, mappingInfo.PdoNumber);
                    break;
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error processing TPDO{Number} mapping for object 0x{Index:X4}:{SubIndex}",
                mappingInfo.PdoNumber, mappingInfo.ObjectIndex, mappingInfo.SubIndex);
        }
    }

    /// <summary>
    /// Đọc giá trị từ object - tự động chọn PDO nếu mapped, fallback SDO
    /// </summary>
    private async Task<T> ReadObjectAsync<T>(ushort index, byte subIndex, CancellationToken ct) where T : struct
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized");

        // Kiểm tra xem object có mapped vào TPDO không
        var key = (index, subIndex);
        if (_pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && !mappingInfo.IsRPDO)
        {
            // Đọc từ cache nếu có
            if (_objectCache.TryGetValue(index, out var cachedValue))
            {
                return CiA402Helper.ConvertBytesToValue<T>(cachedValue, mappingInfo.BitLength);
            }
        }

        // Fallback to SDO read
        return await ReadObjectViaSdoAsync<T>(index, subIndex, ct);
    }

    /// <summary>
    /// Ghi giá trị vào object - tự động chọn RPDO nếu mapped, fallback SDO
    /// </summary>
    private async Task WriteObjectAsync<T>(ushort index, byte subIndex, T value, CancellationToken ct) where T : struct
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized");

        // Kiểm tra xem object có mapped vào RPDO không
        var key = (index, subIndex);
        if (_pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && mappingInfo.IsRPDO)
        {
            // Ghi qua RPDO
            await WriteObjectViaRpdoAsync(index, subIndex, value, mappingInfo, ct);
            return;
        }

        // Fallback to SDO write
        await WriteObjectViaSdoAsync(index, subIndex, value, ct);
    }

    /// <summary>
    /// Đọc object qua SDO
    /// </summary>
    private async Task<T> ReadObjectViaSdoAsync<T>(ushort index, byte subIndex, CancellationToken ct) where T : struct
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized");

        var type = typeof(T);
        if (type == typeof(ushort))
        {
            var value = await _canOpenDevice.ReadUInt16Async(index, subIndex, ct);
            return (T)(object)value;
        }
        else if (type == typeof(short))
        {
            var value = await _canOpenDevice.ReadInt16Async(index, subIndex, ct);
            return (T)(object)value;
        }
        else if (type == typeof(uint))
        {
            var value = await _canOpenDevice.ReadUInt32Async(index, subIndex, ct);
            return (T)(object)value;
        }
        else if (type == typeof(int))
        {
            var value = await _canOpenDevice.ReadInt32Async(index, subIndex, ct);
            return (T)(object)value;
        }
        else if (type == typeof(byte))
        {
            var value = await _canOpenDevice.ReadUInt8Async(index, subIndex, ct);
            return (T)(object)value;
        }
        else if (type == typeof(sbyte))
        {
            // Read as byte and cast to sbyte
            var value = await _canOpenDevice.ReadUInt8Async(index, subIndex, ct);
            return (T)(object)(sbyte)value;
        }

        throw new NotSupportedException($"Type {type.Name} is not supported for SDO read");
    }

    /// <summary>
    /// Ghi object qua SDO
    /// </summary>
    private async Task WriteObjectViaSdoAsync<T>(ushort index, byte subIndex, T value, CancellationToken ct) where T : struct
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized");

        var type = typeof(T);
        if (type == typeof(ushort))
        {
            await _canOpenDevice.WriteUInt16Async(index, subIndex, (ushort)(object)value!, ct);
        }
        else if (type == typeof(short))
        {
            await _canOpenDevice.WriteInt16Async(index, subIndex, (short)(object)value!, ct);
        }
        else if (type == typeof(uint))
        {
            await _canOpenDevice.WriteUInt32Async(index, subIndex, (uint)(object)value!, ct);
        }
        else if (type == typeof(int))
        {
            await _canOpenDevice.WriteInt32Async(index, subIndex, (int)(object)value!, ct);
        }
        else if (type == typeof(byte))
        {
            await _canOpenDevice.WriteUInt8Async(index, subIndex, (byte)(object)value!, ct);
        }
        else if (type == typeof(sbyte))
        {
            // Write sbyte as byte
            await _canOpenDevice.WriteUInt8Async(index, subIndex, (byte)(sbyte)(object)value!, ct);
        }
        else
        {
            throw new NotSupportedException($"Type {type.Name} is not supported for SDO write");
        }
    }

    /// <summary>
    /// Ghi object qua RPDO
    /// </summary>
    private async Task WriteObjectViaRpdoAsync<T>(ushort index, byte subIndex, T value, PdoMappingInfo mappingInfo, CancellationToken ct) where T : struct
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized");

        // Convert value to bytes
        byte[] valueBytes = CiA402Helper.ConvertValueToBytes(value, mappingInfo.BitLength);

        // Nếu RPDO chỉ có một mapping, gửi trực tiếp
        // Nếu có nhiều mappings, cần đọc các giá trị khác và gộp lại
        // Để đơn giản, chỉ gửi nếu RPDO chỉ có mapping này
        var rpdoMappings = _pdoMappingRegistry.Values
            .Where(m => m.IsRPDO && m.PdoNumber == mappingInfo.PdoNumber)
            .OrderBy(m => m.BitOffset)
            .ToList();

        if (rpdoMappings.Count == 1)
        {
            // Single mapping - send directly
            await _canOpenDevice.Pdo.SendRPDOAsync(mappingInfo.PdoNumber, valueBytes, ct);
        }
        else
        {
            // Multiple mappings - need to construct full PDO data
            // For now, fallback to SDO
            await WriteObjectViaSdoAsync(index, subIndex, value, ct);
        }
    }

    private void UpdateProperties()
    {
        try
        {
            lock (_lock)
            {
                SetProperty("Statusword", $"0x{_cachedStatusword.Value:X4}");
                SetProperty("Position", _cachedPosition.ToString());
                SetProperty("Velocity", _cachedVelocity.ToString());
                SetProperty("Torque", _cachedTorque.ToString());
                
                var state = _cachedStatusword.GetState();
                SetProperty("DriveState", state.ToString());
            }
        }
        catch
        {
            // Ignore errors
        }
    }

    #region ICiA402Servo Implementation

    // Cached values
    public Statusword CachedStatusword
    {
        get { lock (_lock) { return _cachedStatusword; } }
    }

    public int CachedPosition
    {
        get { lock (_lock) { return _cachedPosition; } }
    }

    public int CachedVelocity
    {
        get { lock (_lock) { return _cachedVelocity; } }
    }

    public short CachedTorque
    {
        get { lock (_lock) { return _cachedTorque; } }
    }

    // Statusword & Controlword
    public async Task<Statusword> GetStatuswordAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        // Try cached value first (từ TPDO)
        // Kiểm tra xem có mapped vào TPDO không để quyết định có dùng cache không
        var key = (_objectDictionary.Statusword, (byte)0);
        bool isMappedToTpdo = _pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && !mappingInfo.IsRPDO;
        
        if (isMappedToTpdo)
        {
            lock (_lock)
            {
                // Nếu mapped vào TPDO, luôn dùng cache (kể cả khi = 0)
                return _cachedStatusword;
            }
        }

        // Đọc qua PDO/SDO tự động
        var value = await ReadObjectAsync<ushort>(_objectDictionary.Statusword, 0, ct);
        lock (_lock)
        {
            _cachedStatusword = new Statusword(value);
            return _cachedStatusword;
        }
    }

    public async Task SetControlwordAsync(Controlword controlword, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        // Ghi qua RPDO/SDO tự động
        await WriteObjectAsync(_objectDictionary.Controlword, 0, controlword.Value, ct);
    }

    public async Task<DriveState> GetStateAsync(CancellationToken ct = default)
    {
        var statusword = await GetStatuswordAsync(ct);
        return statusword.GetState();
    }

    // Operation Mode
    public async Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        await _canOpenDevice.WriteObjectAsync(_objectDictionary.ModesOfOperation, 0, [(byte)mode], ct);
    }

    public async Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        var value = await _canOpenDevice.ReadUInt8Async(_objectDictionary.ModesOfOperationDisplay, 0, ct);
        return (OperationMode)(sbyte)value;
    }

    // State Machine Control
    public async Task FaultResetAsync(CancellationToken ct = default)
    {
        await SetControlwordAsync(Controlword.FaultResetCmd, ct);
        await Task.Delay(100, ct);
        await SetControlwordAsync(new Controlword(0x0000), ct);
    }

    public async Task ShutdownAsync(CancellationToken ct = default)
    {
        await SetControlwordAsync(Controlword.Shutdown, ct);
    }

    public async Task SwitchOnAsync(CancellationToken ct = default)
    {
        await SetControlwordAsync(Controlword.SwitchOnCmd, ct);
    }

    public async Task EnableOperationAsync(CancellationToken ct = default)
    {
        await SetControlwordAsync(Controlword.EnableOperationCmd, ct);
    }

    public async Task DisableOperationAsync(CancellationToken ct = default)
    {
        await SetControlwordAsync(Controlword.DisableOperation, ct);
    }

    public async Task QuickStopAsync(CancellationToken ct = default)
    {
        await SetControlwordAsync(Controlword.QuickStopCmd, ct);
    }

    public async Task EnableAsync(CancellationToken ct = default)
    {
        var state = await GetStateAsync(ct);

        if (state == DriveState.Fault)
        {
            await FaultResetAsync(ct);
            await Task.Delay(200, ct);
            state = await GetStateAsync(ct);
        }

        if (state == DriveState.SwitchOnDisabled || state == DriveState.NotReadyToSwitchOn)
        {
            await ShutdownAsync(ct);
            await Task.Delay(100, ct);
            state = await GetStateAsync(ct);
        }

        if (state == DriveState.ReadyToSwitchOn)
        {
            await SwitchOnAsync(ct);
            await Task.Delay(100, ct);
            state = await GetStateAsync(ct);
        }

        if (state == DriveState.SwitchedOn)
        {
            await SetControlwordAsync(Controlword.EnableOperationCmd, ct);
            await Task.Delay(100, ct);
        }
    }

    public async Task DisableAsync(CancellationToken ct = default)
    {
        await DisableOperationAsync(ct);
    }

    // Position Control
    public async Task<int> GetActualPositionAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        lock (_lock)
        {
            return _cachedPosition;
        }
    }

    public async Task SetTargetPositionAsync(int position, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        lock (_lock)
        {
            _targetPosition = position;
        }
        
        // Ghi qua RPDO/SDO tự động
        await WriteObjectAsync(_objectDictionary.TargetPosition, 0, (uint)position, ct);
    }
    
    public async Task SetProfileSpeedAsync(uint velocity, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        await _canOpenDevice.WriteUInt32Async(_objectDictionary.ProfileSpeed, 0, velocity, ct);
    }
    
    public async Task SetProfileVelocityAsync(uint velocity, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        await _canOpenDevice.WriteUInt32Async(_objectDictionary.TargetVelocity, 0, velocity, ct);
    }

    public async Task SetProfileAccelerationAsync(uint acceleration, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        await _canOpenDevice.WriteUInt32Async(_objectDictionary.ProfileAcceleration, 0, acceleration, ct);
    }

    public async Task SetProfileDecelerationAsync(uint deceleration, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        await _canOpenDevice.WriteUInt32Async(_objectDictionary.ProfileDeceleration, 0, deceleration, ct);
    }

    public async Task<uint> GetProfileSpeedAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        return await _canOpenDevice.ReadUInt32Async(_objectDictionary.ProfileSpeed, 0, ct);
    }

    public async Task<uint> GetProfileAccelerationAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        return await _canOpenDevice.ReadUInt32Async(_objectDictionary.ProfileAcceleration, 0, ct);
    }

    public async Task<uint> GetProfileDecelerationAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        return await _canOpenDevice.ReadUInt32Async(_objectDictionary.ProfileDeceleration, 0, ct);
    }

    public async Task StartPositionMoveAsync(CancellationToken ct = default)
    {
        var newSetpoint = new Controlword(0x001F);
        var resetSetpoint = new Controlword(0x000F);
        await SetControlwordAsync(newSetpoint, ct);
        await Task.Delay(50, ct);
        await SetControlwordAsync(resetSetpoint, ct);
    }

    public async Task MoveToPositionAsync(int position, uint velocity = 10000, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
    {
        _logger?.LogInformation("CiA402Servo[{DeviceId}]: MoveToPositionAsync position={Position}, velocity={Velocity} -> writing to drive (SDO/PDO).", DeviceId, position, velocity);

        OperationMode currentMode;
        DriveState currentState;
        try
        {
            currentMode = await GetOperationModeAsync(ct);
            currentState = await GetStateAsync(ct);
        }
        catch
        {
            currentMode = OperationMode.ProfilePosition;
            currentState = DriveState.SwitchedOn;
        }

        bool alreadyReady = currentMode == OperationMode.ProfilePosition && currentState == DriveState.OperationEnabled;

        if (!alreadyReady)
        {
            await DisableOperationAsync(ct);
            await SetOperationModeAsync(OperationMode.ProfilePosition, ct);
            await Task.Delay(50, ct);
            await EnableAsync(ct);
            await Task.Delay(50, ct);
        }

        await SetProfileSpeedAsync(velocity, ct);
        await SetProfileAccelerationAsync(acceleration, ct);
        await SetProfileDecelerationAsync(deceleration, ct);
        // Set target trước (giống bấm SetTarget trên device UI)
        await SetTargetPositionAsync(position, ct);
        // Delay để drive latch target trước khi nhận lệnh Start (new setpoint bit) — giống bấm SetTarget rồi bấm Start
        await Task.Delay(150, ct);
        await StartPositionMoveAsync(ct);
    }

    // Velocity Control
    public async Task<int> GetActualVelocityAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        lock (_lock)
        {
            return _cachedVelocity;
        }
    }

    public async Task SetTargetVelocityAsync(int velocity, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        // Ghi qua RPDO/SDO tự động
        await WriteObjectAsync(_objectDictionary.TargetVelocity, 0, velocity, ct);
    }

    public async Task TargetVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
    {
        await SetOperationModeAsync(OperationMode.ProfileVelocity, ct);
        await EnableAsync(ct);
        await SetProfileAccelerationAsync(acceleration, ct);
        await SetProfileDecelerationAsync(deceleration, ct);
        await SetTargetVelocityAsync(targetVelocity, ct);
    }

    public async Task ProfileVelocityAsync(int targetVelocity, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
    {
        await SetOperationModeAsync(OperationMode.ProfileVelocity, ct);
        await Task.Delay(50, ct);
        await EnableAsync(ct);
        await SetProfileAccelerationAsync(acceleration, ct);
        await SetProfileDecelerationAsync(deceleration, ct);
        await SetTargetVelocityAsync(targetVelocity, ct);
    }
    
    // Torque Control
    public async Task<short> GetActualTorqueAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        // Kiểm tra xem có mapped vào TPDO không để quyết định có dùng cache không
        var key = (_objectDictionary.TorqueActualValue, (byte)0);
        bool isMappedToTpdo = _pdoMappingRegistry.TryGetValue(key, out var mappingInfo) && !mappingInfo.IsRPDO;
        
        if (isMappedToTpdo)
        {
            lock (_lock)
            {
                // Nếu mapped vào TPDO, luôn dùng cache (kể cả khi = 0)
                return _cachedTorque;
            }
        }

        // Đọc qua PDO/SDO tự động
        var value = await ReadObjectAsync<ushort>(_objectDictionary.TorqueActualValue, 0, ct);
        lock (_lock)
        {
            _cachedTorque = (short)value;
            return _cachedTorque;
        }
    }

    public async Task SetTargetTorqueAsync(short torque, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        // Ghi qua RPDO/SDO tự động
        await WriteObjectAsync(_objectDictionary.TargetTorque, 0, (ushort)torque, ct);
    }

    public async Task RunTorqueAsync(short torque, CancellationToken ct = default)
    {
        await SetOperationModeAsync(OperationMode.ProfileTorque, ct);
        await Task.Delay(50, ct);
        await SetTargetTorqueAsync(torque, ct);
    }

    /// <summary>
    /// Kiểm tra xem homing method có được phép sử dụng không
    /// </summary>
    private bool IsHomingMethodAllowed(byte method)
    {
        // Nếu không có danh sách giới hạn, cho phép tất cả
        if (_allowedHomingMethods == null || _allowedHomingMethods.Count == 0)
            return true;
        
        return _allowedHomingMethods.Contains(method);
    }

    // Homing
    public async Task SetHomingMethodAsync(byte method, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        // Kiểm tra method có được phép không
        if (!IsHomingMethodAllowed(method))
        {
            var allowedMethods = _allowedHomingMethods != null && _allowedHomingMethods.Count > 0
                ? string.Join(", ", _allowedHomingMethods.OrderBy(m => m))
                : "none";
            throw new ArgumentException($"Homing method {method} is not allowed. Allowed methods: {allowedMethods}", nameof(method));
        }

        // Đảm bảo device ở PreOperational state trước khi set homing method
        // await EnsurePreOperationalStateAsync(ct);
        await DisableOperationAsync(ct);
        // Ghi homing method xuống device
        await _canOpenDevice.WriteUInt8Async(_objectDictionary.HomingMethod, 0, method, ct);
    }

    public async Task SetHomingSpeedAsync(int speed, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        // Sub-index 1: speed during search for switch (CiA402 0x6099)
        await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingSpeed, 1, speed, ct);
    }

    public async Task SetHomingOffsetAsync(int offset, CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        // 0x607C Home Offset - sub-index 0
        await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingOffset, 0, offset, ct);
    }

    public async Task<byte> GetHomingMethodAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        return await _canOpenDevice.ReadUInt8Async(_objectDictionary.HomingMethod, 0, ct);
    }

    public async Task<int> GetHomingSpeedAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        return await _canOpenDevice.ReadInt32Async(_objectDictionary.HomingSpeed, 1, ct);
    }

    public async Task<int> GetHomingOffsetAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");
        return await _canOpenDevice.ReadInt32Async(_objectDictionary.HomingOffset, 0, ct);
    }

    public async Task StartHomingAsync(byte method, int speed, CancellationToken ct = default)
    {
        _logger?.LogInformation("CiA402Servo[{DeviceId}]: StartHomingAsync method={Method}, speed={Speed} -> writing to drive (SDO/Controlword).", DeviceId, method, speed);
        // Kiểm tra method có được phép không
        if (!IsHomingMethodAllowed(method))
        {
            var allowedMethods = _allowedHomingMethods != null && _allowedHomingMethods.Count > 0
                ? string.Join(", ", _allowedHomingMethods.OrderBy(m => m))
                : "none";
            throw new ArgumentException($"Homing method {method} is not allowed. Allowed methods: {allowedMethods}", nameof(method));
        }

        if (_canOpenDevice == null || _objectDictionary == null)
            throw new InvalidOperationException("Device not initialized");

        const int pollIntervalMs = 50;
        const int timeoutMs = 5000;

        // --- Bước 1: PreOperational, ghi đủ tham số homing (giống driver cũ: method, speed switch, speed zero, acceleration, offset) ---
        var currentNmtState = _canOpenDevice.State;
        try
        {
            await EnsurePreOperationalStateAsync(ct);
            await SetControlwordAsync(new Controlword(0x000F), ct);
            await Task.Delay(50, ct);

            await SetHomingMethodAsync(method, ct);
            // 0x6099 sub-index 1: speed during search for switch
            await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingSpeed, 1, speed, ct);
            // 0x6099 sub-index 2: speed during zero search (dùng cùng giá trị speed)
            await _canOpenDevice.WriteInt32Async(_objectDictionary.HomingSpeed, 2, Math.Abs(speed), ct);
            // 0x609A: Homing acceleration (bắt buộc trên một số drive; default nếu chưa set)
            var homingAccel = (uint)Math.Max(100_000, Math.Abs(speed) * 20);
            await _canOpenDevice.WriteUInt32Async(_objectDictionary.HomingAcceleration, 0, homingAccel, ct);
        }
        finally
        {
            if (currentNmtState == NmtState.Operational)
            {
                await _canOpenDevice.StartNodeAsync(ct);
                await Task.Delay(100, ct);
            }
        }

        // --- Bước 2: Set mode Homing, chờ drive chấp nhận (Modes of operation display = 6) ---
        await SetOperationModeAsync(OperationMode.Homing, ct);
        var modeDeadline = DateTime.UtcNow.AddMilliseconds(timeoutMs);
        while (DateTime.UtcNow < modeDeadline)
        {
            var currentMode = await GetOperationModeAsync(ct);
            if (currentMode == OperationMode.Homing)
                break;
            await Task.Delay(pollIntervalMs, ct);
        }
        await Task.Delay(100, ct);

        // --- Bước 3: Gửi Operation enable (0x000F), chờ Statusword Operation enabled (bit 2) ---
        await SetControlwordAsync(new Controlword(0x000F), ct);
        var enableDeadline = DateTime.UtcNow.AddMilliseconds(timeoutMs);
        while (DateTime.UtcNow < enableDeadline)
        {
            var state = await GetStateAsync(ct);
            if (state == DriveState.OperationEnabled)
                break;
            await Task.Delay(pollIntervalMs, ct);
        }
        await Task.Delay(50, ct);

        // --- Bước 4: Gửi Start homing (bit 4 = 1, 0x001F) ---
        await SetControlwordAsync(new Controlword(0x001F), ct);

        var homingAttained = false;
        var homingError = false;
        try
        {
            // --- Bước 5: Chờ homing xong (HomingAttained hoặc HomingError/timeout) ---
            const int homingCompleteTimeoutMs = 120_000; // 2 phút
            var homingDeadline = DateTime.UtcNow.AddMilliseconds(homingCompleteTimeoutMs);
            while (DateTime.UtcNow < homingDeadline)
            {
                ct.ThrowIfCancellationRequested();
                var sw = await GetStatuswordAsync(ct);
                if (sw.HomingError)
                {
                    homingError = true;
                    break;
                }
                if (sw.HomingAttained)
                {
                    homingAttained = true;
                    break;
                }
                await Task.Delay(pollIntervalMs, ct);
            }
        }
        finally
        {
            // Sau homing (dù thành công, lỗi hay timeout): clear bit 4, chuyển mode về Profile Position để có thể điều khiển pos/vel lại
            await SetControlwordAsync(new Controlword(0x000F), ct);
            await Task.Delay(50, ct);
            await SetOperationModeAsync(OperationMode.ProfilePosition, ct);
        }

        // Báo kết quả cho caller: throw để LiftModuleService biết lỗi/timeout, return = thành công (tránh poll lại HomingAttained vì bit đã bị xóa khi đổi mode)
        if (homingError)
            throw new InvalidOperationException("Homing failed: drive reported homing error (Statusword HomingError)");
        if (!homingAttained)
            throw new TimeoutException("Homing timeout after 120 seconds");
    }

    // Target Position Checking
    public bool IsAtTarget(int tolerance = 100, bool useStatusword = true)
    {
        if (useStatusword)
        {
            lock (_lock)
            {
                return _cachedStatusword.TargetReached;
            }
        }
        else
        {
            lock (_lock)
            {
                return Math.Abs(_cachedPosition - _targetPosition) <= tolerance;
            }
        }
    }
    
    public async Task WaitUntilAtTargetAsync(int tolerance = 100, bool useStatusword = true, int checkIntervalMs = 10, CancellationToken ct = default)
    {
        while (!ct.IsCancellationRequested)
        {
            if (IsAtTarget(tolerance, useStatusword))
                return;
            
            await Task.Delay(checkIntervalMs, ct);
        }
        
        throw new OperationCanceledException("Wait for servo at target was cancelled", ct);
    }
    
    // Error and Status Information
    public async Task<bool> IsInFaultStateAsync(CancellationToken ct = default)
    {
        var state = await GetStateAsync(ct);
        return state == DriveState.Fault;
    }
    
    public async Task<byte> GetErrorRegisterAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized");

        try
        {
            return await _canOpenDevice.ReadUInt8Async(0x1001, 0, ct);
        }
        catch
        {
            return 0;
        }
    }
    
    public async Task<ushort[]> GetErrorHistoryAsync(CancellationToken ct = default)
    {
        if (_canOpenDevice == null)
            throw new InvalidOperationException("Device not initialized");

        try
        {
            byte errorCount = await _canOpenDevice.ReadUInt8Async(0x1003, 0, ct);
            
            if (errorCount == 0) return [];
            
            var errors = new List<ushort>();
            for (byte i = 1; i <= errorCount && i <= 8; i++)
            {
                try
                {
                    ushort errorCode = await _canOpenDevice.ReadUInt16Async(0x1003, i, ct);
                    errors.Add(errorCode);
                }
                catch
                {
                    break;
                }
            }
            
            return [.. errors];
        }
        catch
        {
            return [];
        }
    }
    
    public async Task<ushort> GetLatestErrorCodeAsync(CancellationToken ct = default)
    {
        var errorHistory = await GetErrorHistoryAsync(ct);
        return errorHistory.Length > 0 ? errorHistory[0] : (ushort)0x0000;
    }
    
    public async Task<bool> TryFaultResetAsync(CancellationToken ct = default)
    {
        var state = await GetStateAsync(ct);
        if (state != DriveState.Fault)
            return false;
        
        await FaultResetAsync(ct);
        return true;
    }
    
    public async Task<bool> IsEnabledAsync(CancellationToken ct = default)
    {
        var state = await GetStateAsync(ct);
        return state == DriveState.OperationEnabled;
    }
    
    public async Task<bool> IsReadyAsync(CancellationToken ct = default)
    {
        var state = await GetStateAsync(ct);
        return state == DriveState.ReadyToSwitchOn || state == DriveState.SwitchedOn;
    }

    #endregion

    protected override void Dispose(bool disposing)
    {
        if (disposing)
        {
            if (_canOpenDevice != null)
            {
                _canOpenDevice.PdoReceived -= OnPdoReceived;
            }
        }
        base.Dispose(disposing);
    }
}