using RobotNet10.CANOpen;
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 RobotNet10.RobotApp.Drivers.PhenikaaX;
using System.Collections.Concurrent;

namespace RobotNet10.RobotApp.Drivers.ZLAC;

/// <summary>
/// Trục của motor trên driver ZLAC8015D (1 node CAN điều khiển 2 bánh)
/// </summary>
public enum Zlac8015dAxis
{
    Left,
    Right
}

/// <summary>
/// Cấu hình cho ZLAC8015D driver
/// </summary>
internal class Zlac8015dConfig
{
    public string CanInterface { get; set; } = string.Empty;
    public string NodeId { get; set; } = string.Empty;
    /// <summary>
    /// Trục motor mà instance này đại diện: "Left" hoặc "Right"
    /// </summary>
    public string Axis { get; set; } = string.Empty;
    /// <summary>
    /// Số count encoder trên một vòng bánh (ZLAC8015D mặc định 4096)
    /// Dùng để quy đổi counts/s (interface ICiA402Servo) sang r/min (đơn vị của drive)
    /// </summary>
    public int CountsPerRevolution { get; set; } = 4096;
    /// <summary>
    /// Độ phân giải tốc độ đặt (object 0x2026 sub 06): đơn vị lệnh 0x60FF = 1/N r/min.
    /// N = 1..10. Mặc định 10 (0.1 rpm). Nếu firmware không hỗ trợ, driver tự fallback về 1.
    /// </summary>
    public int SpeedResolution { get; set; } = 10;
    /// <summary>
    /// Thời gian tăng tốc (0x6083, ms, 0-32767). ZLAC dùng đơn vị thời gian,
    /// không quy đổi được từ counts/s² nên cấu hình trực tiếp tại đây.
    /// </summary>
    public ushort AccelTimeMs { get; set; } = 200;
    /// <summary>
    /// Thời gian giảm tốc (0x6084, ms, 0-32767)
    /// </summary>
    public ushort DecelTimeMs { get; set; } = 200;
    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; }
}

/// <summary>
/// Driver cho ZLAC8015D dual-wheel hub servo (CANopen CiA402).
/// Đặc thù: MỘT node CAN điều khiển HAI bánh, các object trục dùng sub-index 01 (trái) / 02 (phải);
/// Controlword/Statusword/Mode (0x6040/0x6041/0x6060) dùng chung sub 0 cho cả hai bánh.
///
/// Để giữ nguyên mô hình "mỗi bánh một ICiA402Servo" của DifferentialDrive/OdometryService,
/// mỗi instance của driver này là một "facade" cho MỘT trục (Axis = Left/Right);
/// hai instance (trái + phải) trỏ cùng CanInterface + NodeId sẽ dùng chung một CanOpenDevice
/// (ICanOpenManager cache theo interface + nodeId).
///
/// Đơn vị: interface nhận/trả counts và counts/s (giống driver PhenikaaX);
/// driver quy đổi sang r/min của ZLAC theo CountsPerRevolution.
/// Lệnh 0x60FF ghi theo độ phân giải 1/SpeedResolution r/min (0x2026:06);
/// feedback 0x606C cố định 0.1 r/min; feedback 0x6064 là counts (không quy đổi).
///
/// Feedback qua TPDO. Firmware ZLAC8015D phát TPDO khi DỮ LIỆU THAY ĐỔI (bỏ qua event
/// timer 0x18xx:05 — đo thực tế: TPDO đặt statusword ở đầu không bao giờ phát dù position
/// phía sau đổi liên tục, còn TPDO velocity phát ~240 lần/s theo mỗi thay đổi). Vì vậy:
///   TPDO1 = 0x6064:01 + 0x6064:02 (position 2 bánh - đổi khi lăn bánh -> có event)
///   TPDO2 = 0x6041:00             (statusword    - phát ngay khi đổi, vd fault)
///   TPDO3 = 0x606C:01 + 0x606C:02 (velocity 2 bánh)
/// Mỗi TPDO đặt InhibitTime 50ms để firmware không phát dồn dập theo từng thay đổi.
/// Hai instance trái/phải đăng ký mapping GIỐNG HỆT nhau (idempotent), mỗi instance
/// chỉ đọc phần bit của trục mình. Khi TPDO không phát (đứng yên), poll SDO nền làm tươi.
/// </summary>
[Device(DeviceType.CiA402Servo, "ZLAC", "ZLAC8015D", "1.0.0", Description = "ZLAC8015D Dual Wheel Hub Servo Driver (CANopen CiA402, 1 node / 2 bánh)")]
public class Zlac8015dServo : DeviceBase, ICiA402Servo, IDualAxisVelocityCommand
{
    // Object dictionary ZLAC8015D
    private const ushort ObjControlword = 0x6040;          // sub 0, chung 2 bánh
    private const ushort ObjStatusword = 0x6041;           // sub 0, chung 2 bánh
    private const ushort ObjModesOfOperation = 0x6060;     // sub 0, chung 2 bánh
    private const ushort ObjModesOfOperationDisplay = 0x6061;
    private const ushort ObjPositionActual = 0x6064;       // sub 1/2, counts
    private const ushort ObjVelocityActual = 0x606C;       // sub 1/2, 0.1 r/min
    private const ushort ObjTargetTorque = 0x6071;         // sub 1/2, mA
    private const ushort ObjTargetPosition = 0x607A;       // sub 1/2, counts
    private const ushort ObjProfileVelocity = 0x6081;      // sub 1/2, r/min (position mode)
    private const ushort ObjProfileAcceleration = 0x6083;  // sub 1/2, ms
    private const ushort ObjProfileDeceleration = 0x6084;  // sub 1/2, ms
    private const ushort ObjTargetVelocity = 0x60FF;       // sub 1/2, 1/SpeedResolution r/min
    private const ushort ObjProducerHeartbeatTime = 0x1017; // sub 0, ms - node tự phát heartbeat
    private const ushort HeartbeatProducerTimeMs = 500;     // phải nhỏ hơn HeartbeatConsumerTimeoutMs
    private const int HeartbeatConsumerTimeoutMs = 2000;
    private const int HeartbeatStartupWaitMs = 1500;        // ~3 chu kỳ producer
    private const int EnableTimeoutMs = 3000;               // tổng thời gian đưa drive lên OperationEnabled
    private const int StateStepDelayMs = 100;               // chờ giữa mỗi bước chuyển trạng thái
    // RPDO gộp target velocity của CẢ HAI trục vào MỘT khung: 0x60FF:01 (byte 0-3) +
    // 0x60FF:02 (byte 4-7). Một frame lệnh cả hai bánh cùng lúc -> đồng bộ tuyệt đối,
    // và không đi qua semaphore SDO nên không bị poll feedback chen ngang.
    private const byte DualVelocityRpdoNumber = 1;          // RPDO1, COB-ID 0x200 + nodeId
    private const ushort ObjSyncControlFlag = 0x200F;      // 0 = asynchronous control
    private const ushort ObjMotorParams = 0x2026;          // sub 6 = given speed resolution (1..10)
    private const byte SubSpeedResolution = 0x06;

    // Feedback 0x606C luôn là 0.1 r/min bất kể 0x2026:06
    private const double FeedbackUnitRpm = 0.1;
    private const int MaxRpm = 1000;

    private readonly ICanOpenManager? _canOpenManager;
    private readonly string _canInterface;
    private readonly byte _nodeId;
    private readonly Zlac8015dAxis _axis;
    private readonly byte _axisSub;                 // sub-index của trục: 1 = trái, 2 = phải
    private readonly int _countsPerRevolution;
    private readonly int _configuredSpeedResolution;
    private int _speedResolution;                   // hiệu lực thực tế (fallback = 1 nếu drive từ chối 0x2026:06)
    private readonly ushort _accelTimeMs;
    private readonly ushort _decelTimeMs;
    private readonly int _pdoConfigRetryCount;
    private readonly int _pdoConfigRetryTimeoutMs;
    private readonly bool _useHeartbeatCheck;
    // Trạng thái runtime của heartbeat check: tắt nếu không ghi được 0x1017 (drive không phát heartbeat)
    private volatile bool _heartbeatCheckActive;
    private readonly ILogger _logger;
    private CanOpenDevice? _canOpenDevice;

    // TPDO layout dùng chung cho cả 2 trục (xem doc comment của class)
    private const byte PositionTpdoNumber = 1;      // 0x6064:01 + 0x6064:02
    private const byte StatuswordTpdoNumber = 2;    // 0x6041:00
    private const byte VelocityTpdoNumber = 3;      // 0x606C:01 + 0x606C:02
    // Firmware phát TPDO theo thay đổi dữ liệu, inhibit time chặn phát dồn dập (đơn vị 100µs)
    private const ushort TpdoInhibitTime100Us = 500; // 50ms

    // Registry mapping TPDO của riêng trục này: (index, subIndex) -> info
    private readonly ConcurrentDictionary<(ushort Index, byte SubIndex), PdoMappingInfo> _pdoMappingRegistry = new();

    // Kế hoạch parse TPDO tính sẵn theo PdoNumber. OnPdoReceived chạy đồng bộ trên thread
    // nhận frame CAN (PdoManager.OnFrameReceived) nên tuyệt đối không LINQ/cấp phát mỗi frame.
    private Dictionary<byte, (PdoMappingInfo[] Mappings, int RequiredBytes)> _tpdoParsePlan = new();

    // Thread-safe cached values (đơn vị interface: counts, counts/s)
    private readonly Lock _lock = new();
    private Statusword _cachedStatusword;
    private int _cachedPosition;
    private int _cachedVelocity;

    // Thời điểm TPDO cập nhật từng giá trị lần cuối. Chỉ tin cache khi TPDO còn "tươi";
    // nếu TPDO không về (mapping lỗi/chưa Operational) thì đọc thẳng SDO. Không có
    // fallback này thì Position/Velocity đứng yên ở 0 vĩnh viễn dù drive vẫn chạy.
    private DateTime _statuswordUpdatedAt = DateTime.MinValue;
    private DateTime _positionUpdatedAt = DateTime.MinValue;
    private DateTime _velocityUpdatedAt = DateTime.MinValue;
    private static readonly TimeSpan TpdoCacheTtl = TimeSpan.FromMilliseconds(200);

    // Poll nền làm tươi feedback khi TPDO không về (xem StartFeedbackPolling)
    private const int FeedbackPollIntervalMs = 50;
    private static readonly TimeSpan TpdoAbsentGracePeriod = TimeSpan.FromSeconds(3);
    private CancellationTokenSource? _feedbackPollCts;
    private Task? _feedbackPollTask;
    private volatile bool _feedbackPollWarned;
    private DateTime? _tpdoAbsentSinceUtc;
    private short _cachedTorque;
    private int _targetPosition;

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

    public Zlac8015dServo(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
        : base(deviceId, deviceName, DeviceType.CiA402Servo)
    {
        var config = new Zlac8015dConfig();
        connection.Bind(config);

        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");

        if (!Enum.TryParse(config.Axis, ignoreCase: true, out _axis))
            throw new InvalidOperationException($"Invalid Axis: '{config.Axis}'. Must be 'Left' or 'Right'");

        _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");

        if (config.CountsPerRevolution <= 0)
            throw new InvalidOperationException($"Invalid CountsPerRevolution: {config.CountsPerRevolution}. Must be greater than 0");

        if (config.SpeedResolution < 1 || config.SpeedResolution > 10)
            throw new InvalidOperationException($"Invalid SpeedResolution: {config.SpeedResolution}. Must be between 1 and 10");

        _countsPerRevolution = config.CountsPerRevolution;
        _configuredSpeedResolution = config.SpeedResolution;
        _speedResolution = config.SpeedResolution;
        _accelTimeMs = config.AccelTimeMs;
        _decelTimeMs = config.DecelTimeMs;

        _axisSub = _axis == Zlac8015dAxis.Left ? (byte)1 : (byte)2;

        _pdoConfigRetryCount = config.PdoConfigRetryCount ?? 3;
        _pdoConfigRetryTimeoutMs = config.PdoConfigRetryTimeoutMs ?? 1000;
        _useHeartbeatCheck = config.UseHeartbeatCheck ?? true;

        AutoReconnectEnabled = config.AutoReconnectEnabled ?? true;
        ReconnectDelayMs = config.ReconnectDelayMs ?? 3000;
        MaxReconnectAttempts = config.MaxReconnectAttempts ?? 0;

        SetProperty("CanInterface", _canInterface);
        SetProperty("NodeId", _nodeId.ToString());
        SetProperty("Axis", _axis.ToString());
        SetProperty("UseHeartbeatCheck", _useHeartbeatCheck.ToString());

        _canOpenManager = serviceProvider.GetRequiredService<ICanOpenManager>();
        _logger = serviceProvider.GetRequiredService<ILoggerFactory>().CreateLogger<Zlac8015dServo>();

        _cachedStatusword = new Statusword(0);
    }

    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), chung cho cả 2 bánh")
        {
            DataType = "number",
            IsReadOnly = true,
            DisplayOrder = 2,
            Category = "Kết nối",
            DefaultValue = "1"
        };

        yield return new PropertyDescription("Axis", "Trục", "Trục motor: Left hoặc Right")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 3,
            Category = "Kết nối",
            DefaultValue = "Left"
        };

        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, chung 2 bánh)")
        {
            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("DriveState", "Drive State", "Trạng thái drive")
        {
            DataType = "string",
            IsReadOnly = true,
            DisplayOrder = 8,
            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.");
        }

        // CanOpenManager cache theo (interface, nodeId) nên 2 instance trái/phải nhận cùng một CanOpenDevice
        _canOpenDevice = await _canOpenManager.GetOrCreateDeviceAsync(_canInterface, _nodeId, cancellationToken);

        _canOpenDevice.PdoReceived += OnPdoReceived;

        ConfigurePdos();
        UpdateProperties();
    }

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

        if (_canOpenManager != null)
        {
            var canBus = _canOpenManager.GetCanBus(_canInterface);
            if (canBus == null || !canBus.IsConnected)
            {
                throw new InvalidOperationException($"ICanBus for interface '{_canInterface}' is not connected.");
            }
        }

        // Hai facade trái/phải dùng chung node vật lý: giữ lock để trình tự
        // PDO config + NMT + tham số của trục này không xen kẽ với trục kia
        await _canOpenDevice.ConfigurationLock.WaitAsync(cancellationToken);
        try
        {
            await ConfigureDevicePdosAsync(cancellationToken);

            var currentState = _canOpenDevice.State;
            if (currentState != NmtState.PreOperational && currentState != NmtState.Stopped)
            {
                await _canOpenDevice.SendNmtCommandAsync(NmtCommand.PreOperational, cancellationToken);
                await Task.Delay(100, cancellationToken);
            }

            // Bật heartbeat SAU khi cấu hình PDO: PdoManager gửi NMT Reset Communication,
            // lệnh này reset vùng object 0x1000-0x1FFF về mặc định (0x1017 = 0) nên nếu ghi
            // trước thì heartbeat producer bị xóa và node không bao giờ phát heartbeat.
            if (_useHeartbeatCheck)
            {
                await EnableHeartbeatAsync(cancellationToken);
            }

            await _canOpenDevice.StartNodeAsync(cancellationToken);
            await VerifyOperationalAsync(cancellationToken);

            await ConfigureDriveParametersAsync(cancellationToken);

            bool rpdoReady = _canOpenDevice.Pdo.IsRpdoConfigured(DualVelocityRpdoNumber);
            _logger.LogInformation(
                "ZLAC8015D Node {NodeId} axis {Axis}: lệnh vận tốc đồng bộ qua RPDO1 = {State}. {Detail}",
                _nodeId, _axis, rpdoReady ? "BẬT" : "TẮT",
                rpdoReady
                    ? "Hai bánh nhận lệnh trong cùng một khung CAN."
                    : "Firmware không nhận RPDO -> fallback ghi SDO tuần tự (có thể lệch nhẹ).");
        }
        finally
        {
            _canOpenDevice.ConfigurationLock.Release();
        }

        StartFeedbackPolling();
        UpdateProperties();
    }

    /// <summary>
    /// Ghi 0x1017 để drive tự phát heartbeat, bật monitor phía consumer rồi chờ heartbeat đầu tiên về.
    /// Nếu drive không phát (firmware không hỗ trợ 0x1017, hoặc bị Reset Communication xóa) thì
    /// tắt heartbeat check và fallback sang đọc statusword — nếu không, IsAlive không bao giờ true
    /// và device sẽ fail connect vĩnh viễn.
    /// </summary>
    private async Task EnableHeartbeatAsync(CancellationToken ct)
    {
        try
        {
            await _canOpenDevice!.WriteUInt16Async(ObjProducerHeartbeatTime, 0, HeartbeatProducerTimeMs, ct);
            _canOpenDevice.AutoVerifyStateViaHeartbeat = true;
            _canOpenDevice.EnableHeartbeatMonitoring(timeoutMs: HeartbeatConsumerTimeoutMs);

            if (await WaitForFirstHeartbeatAsync(ct))
            {
                _heartbeatCheckActive = true;
                return;
            }

            _heartbeatCheckActive = false;
            _canOpenDevice.AutoVerifyStateViaHeartbeat = false;
            _canOpenDevice.DisableHeartbeatMonitoring();
            _logger.LogWarning(
                "ZLAC8015D Node {NodeId}: đã ghi 0x1017 = {TimeMs}ms nhưng không nhận được heartbeat sau {WaitMs}ms. " +
                "Tắt kiểm tra heartbeat, fallback đọc statusword.",
                _nodeId, HeartbeatProducerTimeMs, HeartbeatStartupWaitMs);
        }
        catch (Exception ex)
        {
            _heartbeatCheckActive = false;
            _logger.LogWarning(ex,
                "ZLAC8015D Node {NodeId}: không ghi được 0x1017 (producer heartbeat = {TimeMs}ms), tắt kiểm tra heartbeat, fallback đọc statusword",
                _nodeId, HeartbeatProducerTimeMs);
        }
    }

    /// <summary>
    /// Xác nhận node thật sự vào Operational sau NMT Start. TPDO CHỈ được phát ở Operational,
    /// nên nếu lệnh Start bị bỏ qua thì mọi cấu hình TPDO đúng đến đâu cũng vô nghĩa.
    /// Trạng thái NMT lấy từ heartbeat (byte dữ liệu của frame 0x700+NodeId).
    /// </summary>
    private async Task VerifyOperationalAsync(CancellationToken ct)
    {
        if (!_heartbeatCheckActive)
            return; // Không có heartbeat thì không quan sát được state, bỏ qua

        const int maxWaitMs = 1500;
        int waited = 0;

        while (waited < maxWaitMs)
        {
            await Task.Delay(100, ct);
            waited += 100;

            var nodeInfo = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId);
            if (nodeInfo?.LastState == NmtState.Operational)
            {
                _logger.LogInformation("ZLAC8015D Node {NodeId} axis {Axis}: node đã vào Operational, TPDO sẽ bắt đầu phát.",
                    _nodeId, _axis);
                return;
            }
        }

        var lastState = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId)?.LastState;
        _logger.LogWarning(
            "ZLAC8015D Node {NodeId} axis {Axis}: sau NMT Start, heartbeat vẫn báo trạng thái {State} (không phải Operational) sau {MaxWaitMs}ms. " +
            "TPDO sẽ không phát — feedback sẽ chạy bằng fallback SDO.",
            _nodeId, _axis, lastState?.ToString() ?? "không rõ", maxWaitMs);
    }

    private async Task<bool> WaitForFirstHeartbeatAsync(CancellationToken ct)
    {
        int waited = 0;
        while (waited < HeartbeatStartupWaitMs)
        {
            await Task.Delay(50, ct);
            waited += 50;

            var nodeInfo = _canOpenDevice!.Heartbeat.GetNodeInfo(_nodeId);
            if (nodeInfo != null && nodeInfo.IsAlive)
                return true;
        }

        return false;
    }

    /// <summary>
    /// Ghi các tham số vận hành của ZLAC8015D sau khi node Operational:
    /// chế độ điều khiển bất đồng bộ, độ phân giải tốc độ, thời gian tăng/giảm tốc.
    /// Hai instance trái/phải ghi trùng các object chung (0x200F, 0x2026) — idempotent, không sao.
    /// </summary>
    private async Task ConfigureDriveParametersAsync(CancellationToken ct)
    {
        if (_canOpenDevice == null) return;

        // 0x200F = 0: asynchronous control (mỗi bánh nhận lệnh độc lập, không chờ frame sync)
        await _canOpenDevice.WriteUInt16Async(ObjSyncControlFlag, 0, 0, ct);

        // 0x2026:06 - độ phân giải tốc độ đặt (1/N r/min). Firmware cũ có thể không hỗ trợ.
        // QUAN TRỌNG: object này là CHUNG cho cả node nhưng _speedResolution là bản riêng của
        // từng instance trái/phải. Nếu một instance ghi lỗi mà tự đoán fallback = 1 trong khi
        // instance kia đã ghi 10 thành công, thì mọi lệnh SDO của trục lệch sẽ nhỏ hơn 10 lần
        // -> một bánh luôn chạy chậm hơn bánh kia. Vì vậy luôn ĐỌC LẠI giá trị thật từ drive
        // để hai instance và drive thống nhất một độ phân giải.
        try
        {
            await _canOpenDevice.WriteUInt16Async(ObjMotorParams, SubSpeedResolution, (ushort)_configuredSpeedResolution, ct);
            _speedResolution = _configuredSpeedResolution;
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex,
                "ZLAC8015D Node {NodeId} axis {Axis}: không ghi được 0x2026:06 (speed resolution = {Resolution}), sẽ đọc lại giá trị thật từ drive",
                _nodeId, _axis, _configuredSpeedResolution);
        }

        try
        {
            ushort actual = await _canOpenDevice.ReadUInt16Async(ObjMotorParams, SubSpeedResolution, ct);
            if (actual >= 1 && actual <= 10)
            {
                if (actual != _speedResolution)
                {
                    _logger.LogWarning(
                        "ZLAC8015D Node {NodeId} axis {Axis}: 0x2026:06 trên drive = {Actual} khác giá trị dự kiến {Expected}, dùng giá trị của drive",
                        _nodeId, _axis, actual, _speedResolution);
                }
                _speedResolution = actual;
            }
        }
        catch (Exception ex)
        {
            // Không đọc được (firmware không hỗ trợ 0x2026:06) -> chắc chắn không ghi được,
            // drive chạy đơn vị mặc định 1 r/min cho cả hai trục nên fallback 1 là nhất quán.
            _speedResolution = 1;
            _logger.LogWarning(ex,
                "ZLAC8015D Node {NodeId} axis {Axis}: không đọc được 0x2026:06, fallback đơn vị lệnh 1 r/min",
                _nodeId, _axis);
        }

        _logger.LogInformation("ZLAC8015D Node {NodeId} axis {Axis}: speed resolution hiệu lực = 1/{Resolution} r/min",
            _nodeId, _axis, _speedResolution);

        // Thời gian tăng/giảm tốc (ms). Ghi cho CẢ HAI trục chứ không chỉ trục mình: khi
        // instance kia connect SAU, bước cấu hình PDO của nó gửi NMT Reset Communication làm
        // drive khôi phục mặc định (đo thực tế: trục đã ghi 200ms bị trả về 10ms) -> hai bánh
        // tăng/giảm tốc lệch nhau 20 lần, một bánh luôn "ì" hơn ở pha quá độ. Hai instance
        // dùng chung AccelTimeMs/DecelTimeMs trong config nên ghi trùng là vô hại.
        for (byte sub = 1; sub <= 2; sub++)
        {
            await _canOpenDevice.WriteUInt32Async(ObjProfileAcceleration, sub, _accelTimeMs, ct);
            await _canOpenDevice.WriteUInt32Async(ObjProfileDeceleration, sub, _decelTimeMs, ct);
        }
    }

    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        StopFeedbackPolling();

        if (_canOpenDevice != null)
        {
            // Dừng bánh của trục này trước khi ngắt (node CAN dùng chung với trục còn lại nên không stop node ở đây)
            try
            {
                await _canOpenDevice.WriteInt32Async(ObjTargetVelocity, _axisSub, 0, cancellationToken);
            }
            catch
            {
                // Ignore - bus có thể đã mất
            }
            _canOpenDevice.DisableHeartbeatMonitoring();
        }
    }

    protected override async Task OnResetAsync(CancellationToken cancellationToken)
    {
        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)
        {
            _logger.LogWarning(ex, "Error occurred during reset for Node {NodeId} axis {Axis}", _nodeId, _axis);
        }

        UpdateProperties();
    }

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

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

            await GetStatuswordAsync(cancellationToken);
            return true;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error checking connection for Node {NodeId} axis {Axis}", _nodeId, _axis);
            return false;
        }
    }

    #region PDO Configuration

    /// <summary>
    /// Đăng ký TPDO feedback cho trục này lên CanOpenDevice dùng chung.
    /// ZLAC8015D chỉ có 3 TPDO (0x1A03 không tồn tại trên firmware). Firmware phát TPDO
    /// theo THAY ĐỔI dữ liệu chứ không theo event timer (đo thực tế trên bus), nên mỗi TPDO
    /// chỉ chở một loại dữ liệu và object hay thay đổi phải đứng đầu:
    ///   TPDO1 = position CẢ HAI trục   (0x6064:01 + 0x6064:02) = 8 byte
    ///   TPDO2 = statusword             (0x6041:00)             = 2 byte
    ///   TPDO3 = velocity CẢ HAI trục   (0x606C:01 + 0x606C:02) = 8 byte
    /// (layout cũ TPDO1/2 = statusword + position từng trục KHÔNG BAO GIỜ phát vì object
    /// đầu tiên - statusword - hầu như không đổi.)
    /// Hai instance cùng đăng ký mapping GIỐNG HỆT nhau (idempotent), mỗi instance chỉ đọc
    /// phần bit của trục mình: trái bit 0-31, phải bit 32-63.
    /// </summary>
    private void ConfigurePdos()
    {
        if (_canOpenDevice == null)
            return;

        _pdoMappingRegistry.Clear();

        // TPDO1 position: 0x6064:01 (bit 0-31) + 0x6064:02 (bit 32-63), counts
        var positionTpdo = new PdoConfiguration(PositionTpdoNumber, (uint)(0x180 + (PositionTpdoNumber - 1) * 0x100 + _nodeId))
        {
            EventTimer = 50,
            InhibitTime = TpdoInhibitTime100Us
        };
        positionTpdo.AddMapping(new PdoMapping(ObjPositionActual, 1, 32));
        positionTpdo.AddMapping(new PdoMapping(ObjPositionActual, 2, 32));
        _canOpenDevice.Pdo.ConfigureTPDO(positionTpdo);

        _pdoMappingRegistry[(ObjPositionActual, _axisSub)] = new PdoMappingInfo
        {
            IsRPDO = false,
            PdoNumber = PositionTpdoNumber,
            BitOffset = (_axisSub - 1) * 32,
            BitLength = 32,
            ObjectIndex = ObjPositionActual,
            SubIndex = _axisSub
        };

        // TPDO2 statusword: 0x6041:00 (16 bit, chung 2 trục). Statusword ít đổi nên TPDO này
        // hiếm khi phát — nhưng khi đổi (vd fault) sẽ phát ngay lập tức; giữa các lần đổi,
        // poll SDO nền vẫn làm tươi định kỳ.
        var statuswordTpdo = new PdoConfiguration(StatuswordTpdoNumber, (uint)(0x180 + (StatuswordTpdoNumber - 1) * 0x100 + _nodeId))
        {
            EventTimer = 50,
            InhibitTime = TpdoInhibitTime100Us
        };
        statuswordTpdo.AddMapping(new PdoMapping(ObjStatusword, 0, 16));
        _canOpenDevice.Pdo.ConfigureTPDO(statuswordTpdo);

        _pdoMappingRegistry[(ObjStatusword, (byte)0)] = new PdoMappingInfo
        {
            IsRPDO = false,
            PdoNumber = StatuswordTpdoNumber,
            BitOffset = 0,
            BitLength = 16,
            ObjectIndex = ObjStatusword,
            SubIndex = 0
        };

        // TPDO3 velocity: 0x606C:01 (bit 0-31) + 0x606C:02 (bit 32-63), đơn vị 0.1 r/min.
        // InhibitTime 50ms để firmware không phát theo từng thay đổi (đo được 240 frame/s khi chạy).
        var velocityTpdo = new PdoConfiguration(VelocityTpdoNumber, (uint)(0x180 + (VelocityTpdoNumber - 1) * 0x100 + _nodeId))
        {
            EventTimer = 50,
            InhibitTime = TpdoInhibitTime100Us
        };
        velocityTpdo.AddMapping(new PdoMapping(ObjVelocityActual, 1, 32));
        velocityTpdo.AddMapping(new PdoMapping(ObjVelocityActual, 2, 32));
        _canOpenDevice.Pdo.ConfigureTPDO(velocityTpdo);

        _pdoMappingRegistry[(ObjVelocityActual, _axisSub)] = new PdoMappingInfo
        {
            IsRPDO = false,
            PdoNumber = VelocityTpdoNumber,
            BitOffset = (_axisSub - 1) * 32,
            BitLength = 32,
            ObjectIndex = ObjVelocityActual,
            SubIndex = _axisSub
        };

        // RPDO1 lệnh vận tốc đồng bộ: 0x60FF:01 (bit 0-31) + 0x60FF:02 (bit 32-63).
        // Cả hai instance đăng ký y hệt nhau (idempotent). Nếu firmware không nhận RPDO
        // này thì TrySetBothTargetVelocitiesAsync trả false và tự fallback về SDO tuần tự.
        var dualVelocityRpdo = new PdoConfiguration(DualVelocityRpdoNumber, (uint)(0x200 + _nodeId));
        dualVelocityRpdo.AddMapping(new PdoMapping(ObjTargetVelocity, 1, 32));
        dualVelocityRpdo.AddMapping(new PdoMapping(ObjTargetVelocity, 2, 32));
        _canOpenDevice.Pdo.ConfigureRPDO(dualVelocityRpdo);

        // Tính sẵn kế hoạch parse cho OnPdoReceived. RequiredBytes theo bit CUỐI CÙNG cần đọc,
        // không phải tổng độ dài: trục phải đọc velocity ở bit 32-63 của TPDO3 nên cần đủ
        // 8 byte dù chỉ map 32 bit.
        _tpdoParsePlan = _pdoMappingRegistry.Values
            .Where(m => !m.IsRPDO)
            .GroupBy(m => m.PdoNumber)
            .ToDictionary(
                g => g.Key,
                g =>
                {
                    var mappings = g.OrderBy(m => m.BitOffset).ToArray();
                    int lastBit = mappings.Max(m => m.BitOffset + m.BitLength);
                    return (mappings, (lastBit + 7) / 8);
                });
    }

    /// <summary>
    /// Ghi PDO configuration xuống device qua SDO (retry giống driver PhenikaaX)
    /// </summary>
    private async Task ConfigureDevicePdosAsync(CancellationToken ct)
    {
        Exception? lastException = null;
        for (int attempt = 1; attempt <= _pdoConfigRetryCount; attempt++)
        {
            try
            {
                await EnsurePreOperationalStateAsync(ct);
                await _canOpenDevice!.WriteAllPdoConfigurationsToDeviceAsync(null, ct);
                return;
            }
            catch (CanOpenTimeoutException timeoutEx)
            {
                lastException = timeoutEx;

                _logger.LogWarning("PDO configuration attempt {Attempt}/{MaxRetries} failed for Node {NodeId} axis {Axis}: {Error}",
                    attempt, _pdoConfigRetryCount, _nodeId, _axis, 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
                    }
                }
            }
        }

        throw new InvalidOperationException(
            $"Failed to configure PDOs after {_pdoConfigRetryCount} attempts for Node {_nodeId} axis {_axis}. " +
            $"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);
        }
    }

    #endregion

    #region PDO Receive

    private void OnPdoReceived(object? sender, PdoReceivedEventArgs e)
    {
        var data = e.Data;

        // Chỉ parse TPDO thuộc trục này (instance của trục kia tự parse TPDO của nó)
        if (!_tpdoParsePlan.TryGetValue(data.PdoNumber, out var plan))
            return;

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

        // Cập nhật cache trong lock, nhưng bắn event NGOÀI lock: handler này chạy đồng bộ
        // trên thread nhận frame CAN, subscriber chậm (UI/odometry) mà chạy trong lock sẽ
        // chặn cả đường nhận CAN (SDO response, heartbeat, TPDO trục kia) -> trễ dây chuyền
        // cả điều khiển lẫn hiển thị.
        StatuswordChangedEventArgs? statuswordArgs = null;
        PositionChangedEventArgs? positionArgs = null;
        VelocityChangedEventArgs? velocityArgs = null;
        ushort oldStatuswordValue = 0;

        lock (_lock)
        {
            foreach (var mappingInfo in plan.Mappings)
            {
                try
                {
                    byte[] valueBytes = CiA402Helper.ExtractValueFromPdoData(data.Data, mappingInfo.BitOffset, mappingInfo.BitLength);

                    switch (mappingInfo.ObjectIndex)
                    {
                        case ObjStatusword when valueBytes.Length >= 2:
                            var newStatusword = new Statusword(BitConverter.ToUInt16(valueBytes, 0));
                            _statuswordUpdatedAt = DateTime.UtcNow;

                            if (newStatusword.Value != _cachedStatusword.Value)
                            {
                                oldStatuswordValue = _cachedStatusword.Value;
                                statuswordArgs = new StatuswordChangedEventArgs(
                                    newStatusword, _cachedStatusword.GetState(), newStatusword.GetState());
                                _cachedStatusword = newStatusword;
                            }
                            break;

                        case ObjPositionActual when valueBytes.Length >= 4:
                            var position = BitConverter.ToInt32(valueBytes, 0);
                            _positionUpdatedAt = DateTime.UtcNow;

                            if (position != _cachedPosition)
                            {
                                positionArgs = new PositionChangedEventArgs(position, _cachedPosition);
                                _cachedPosition = position;
                            }
                            break;

                        case ObjVelocityActual when valueBytes.Length >= 4:
                            var velocity = FeedbackRawToCountsPerSec(BitConverter.ToInt32(valueBytes, 0));
                            _velocityUpdatedAt = DateTime.UtcNow;

                            if (velocity != _cachedVelocity)
                            {
                                velocityArgs = new VelocityChangedEventArgs(velocity, _cachedVelocity);
                                _cachedVelocity = velocity;
                            }
                            break;
                    }
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "Error processing TPDO{Number} mapping for object 0x{Index:X4}:{SubIndex}",
                        mappingInfo.PdoNumber, mappingInfo.ObjectIndex, mappingInfo.SubIndex);
                }
            }
        }

        if (statuswordArgs != null)
        {
            if (statuswordArgs.NewState != statuswordArgs.OldState)
            {
                _logger.LogInformation(
                    "ZLAC8015D Node {NodeId} axis {Axis}: DriveState {OldState} -> {NewState} (statusword 0x{Old:X4} -> 0x{New:X4})",
                    _nodeId, _axis, statuswordArgs.OldState, statuswordArgs.NewState,
                    oldStatuswordValue, statuswordArgs.Statusword.Value);
            }

            StatuswordChanged?.Invoke(this, statuswordArgs);
        }

        if (positionArgs != null)
            PositionChanged?.Invoke(this, positionArgs);

        if (velocityArgs != null)
            VelocityChanged?.Invoke(this, velocityArgs);

        // Chỉ tốn công cập nhật property khi thực sự có giá trị thay đổi
        if (statuswordArgs != null || positionArgs != null || velocityArgs != null)
            UpdateProperties();
    }

    #endregion

    #region Unit Conversion

    /// <summary>
    /// counts/s (đơn vị interface) -> giá trị lệnh 0x60FF (1/SpeedResolution r/min), kẹp ±1000 r/min
    /// </summary>
    private int CountsPerSecToCommandRaw(int countsPerSec)
    {
        double rpm = (double)countsPerSec * 60.0 / _countsPerRevolution;
        double clamped = Math.Clamp(rpm, -MaxRpm, MaxRpm);
        return (int)Math.Round(clamped * _speedResolution);
    }

    /// <summary>
    /// Giá trị feedback 0x606C (0.1 r/min) -> counts/s (đơn vị interface)
    /// </summary>
    private int FeedbackRawToCountsPerSec(int feedbackRaw)
    {
        return (int)Math.Round(feedbackRaw * FeedbackUnitRpm / 60.0 * _countsPerRevolution);
    }

    /// <summary>
    /// counts/s -> r/min nguyên (dùng cho 0x6081 profile velocity, range 1-1000)
    /// </summary>
    private uint CountsPerSecToRpm(uint countsPerSec)
    {
        double rpm = (double)countsPerSec * 60.0 / _countsPerRevolution;
        return (uint)Math.Clamp(Math.Round(rpm), 1, MaxRpm);
    }

    #endregion

    private void UpdateProperties()
    {
        try
        {
            lock (_lock)
            {
                SetProperty("Statusword", $"0x{_cachedStatusword.Value:X4}");
                SetProperty("Position", _cachedPosition.ToString());
                SetProperty("Velocity", _cachedVelocity.ToString());

                var state = _cachedStatusword.GetState();
                SetProperty("DriveState", state.ToString());
            }
        }
        catch
        {
            // Ignore errors
        }
    }

    private CanOpenDevice RequireDevice()
    {
        return _canOpenDevice ?? throw new InvalidOperationException("Device not initialized");
    }

    #region ICiA402Servo Implementation

    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 (0x6040/0x6041 sub 0 - chung cho cả 2 bánh)
    public async Task<Statusword> GetStatuswordAsync(CancellationToken ct = default)
    {
        var device = RequireDevice();

        // Chỉ dùng cache khi TPDO vừa cập nhật trong TTL. Trước đây cache được dùng
        // mãi mãi khi != 0, nên nếu TPDO ngừng về thì GetStateAsync trả giá trị cũ
        // vĩnh viễn và EnableAsync không bao giờ thấy trạng thái tiến triển.
        lock (_lock)
        {
            if (DateTime.UtcNow - _statuswordUpdatedAt < TpdoCacheTtl)
                return _cachedStatusword;
        }

        return await ReadStatuswordViaSdoAsync(device, ct);
    }

    /// <summary>
    /// Đọc statusword trực tiếp qua SDO, bỏ qua cache TPDO. State machine dùng hàm này
    /// để mỗi bước chuyển trạng thái đều thấy giá trị mới nhất từ drive.
    /// </summary>
    private async Task<Statusword> ReadStatuswordViaSdoAsync(CanOpenDevice device, CancellationToken ct)
    {
        var value = await device.ReadUInt16Async(ObjStatusword, 0, ct);
        var statusword = new Statusword(value);

        bool changed;
        Statusword previous;
        lock (_lock)
        {
            previous = _cachedStatusword;
            changed = statusword.Value != previous.Value;
            _cachedStatusword = statusword;
            // Không cập nhật _statuswordUpdatedAt: mốc đó chỉ dành cho TPDO,
            // để lần đọc sau vẫn lấy giá trị tươi nếu TPDO chưa hoạt động.
        }

        if (changed)
        {
            var oldState = previous.GetState();
            var newState = statusword.GetState();

            if (newState != oldState)
            {
                _logger.LogInformation(
                    "ZLAC8015D Node {NodeId} axis {Axis}: DriveState {OldState} -> {NewState} (statusword 0x{Old:X4} -> 0x{New:X4}, đọc qua SDO)",
                    _nodeId, _axis, oldState, newState, previous.Value, statusword.Value);
            }

            StatuswordChanged?.Invoke(this, new StatuswordChangedEventArgs(statusword, oldState, newState));
        }

        return statusword;
    }

    private async Task<DriveState> ReadStateViaSdoAsync(CancellationToken ct)
    {
        var statusword = await ReadStatuswordViaSdoAsync(RequireDevice(), ct);
        return statusword.GetState();
    }

    public async Task SetControlwordAsync(Controlword controlword, CancellationToken ct = default)
    {
        await RequireDevice().WriteUInt16Async(ObjControlword, 0, controlword.Value, ct);
    }

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

    // Operation Mode (0x6060 sub 0 - chung cho cả 2 bánh)
    public async Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default)
    {
        await RequireDevice().WriteUInt8Async(ObjModesOfOperation, 0, (byte)mode, ct);
    }

    public async Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default)
    {
        var value = await RequireDevice().ReadUInt8Async(ObjModesOfOperationDisplay, 0, ct);
        return (OperationMode)(sbyte)value;
    }

    // State Machine Control (chung 2 bánh - enable một trục là enable cả drive)
    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);
    }

    /// <summary>
    /// Đưa drive lên OperationEnabled theo state machine CiA402.
    /// Chạy vòng lặp cho tới khi đạt trạng thái hoặc hết thời gian, mỗi vòng đọc statusword
    /// tươi qua SDO (không tin cache TPDO), và ném exception kèm statusword nếu thất bại —
    /// bản cũ chạy một lượt tuyến tính rồi im lặng bỏ cuộc nếu drive chưa kịp chuyển trạng thái.
    /// </summary>
    public async Task EnableAsync(CancellationToken ct = default)
    {
        var deadline = DateTime.UtcNow + TimeSpan.FromMilliseconds(EnableTimeoutMs);
        DriveState state = DriveState.Unknown;

        while (DateTime.UtcNow < deadline)
        {
            state = await ReadStateViaSdoAsync(ct);

            switch (state)
            {
                case DriveState.OperationEnabled:
                    return;

                case DriveState.NotReadyToSwitchOn:
                    // Trạng thái tự động của CiA402: drive đang tự khởi tạo, KHÔNG nhận lệnh
                    // controlword nào. Chỉ được chờ nó tự chuyển sang SwitchOnDisabled.
                    await Task.Delay(StateStepDelayMs, ct);
                    break;

                case DriveState.Fault:
                    await FaultResetAsync(ct);
                    await Task.Delay(StateStepDelayMs, ct);
                    break;

                case DriveState.SwitchOnDisabled:
                    await ShutdownAsync(ct);            // 0x0006 -> ReadyToSwitchOn
                    await Task.Delay(StateStepDelayMs, ct);
                    break;

                case DriveState.ReadyToSwitchOn:
                    await SwitchOnAsync(ct);            // 0x0007 -> SwitchedOn
                    await Task.Delay(StateStepDelayMs, ct);
                    break;

                case DriveState.SwitchedOn:
                    await EnableOperationAsync(ct);     // 0x000F -> OperationEnabled
                    await Task.Delay(StateStepDelayMs, ct);
                    break;

                default:
                    await Task.Delay(StateStepDelayMs, ct);
                    break;
            }
        }

        var statusword = await ReadStatuswordViaSdoAsync(RequireDevice(), ct);
        throw new InvalidOperationException(
            $"ZLAC8015D Node {_nodeId} axis {_axis}: không đạt được OperationEnabled sau {EnableTimeoutMs}ms. " +
            $"Trạng thái cuối: {state}, statusword = 0x{statusword.Value:X4}. " +
            (statusword.Value == 0
                ? "Statusword = 0 nghĩa là drive không trả về trạng thái hợp lệ — kiểm tra nguồn động lực (motor power) và dây encoder."
                : "Kiểm tra fault code và nguồn động lực của drive."));
    }

    public async Task DisableAsync(CancellationToken ct = default)
    {
        // Đưa tốc độ trục này về 0 trước; controlword là chung nên chỉ disable
        // khi caller thực sự muốn tắt cả drive (DifferentialDrive luôn disable cả 2 bánh)
        try
        {
            await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, 0, ct);
        }
        catch
        {
            // Ignore - vẫn tiếp tục disable
        }
        await DisableOperationAsync(ct);
    }

    // Position Control (0x607A sub theo trục, counts)
    /// <summary>
    /// Vị trí thực tế (counts). Ưu tiên cache TPDO khi còn tươi, ngược lại đọc SDO 0x6064:sub.
    /// Trước đây hàm này CHỈ trả cache TPDO nên khi TPDO không về (mapping lỗi) thì
    /// Position đứng yên ở 0 mãi mãi dù động cơ vẫn quay.
    /// </summary>
    public async Task<int> GetActualPositionAsync(CancellationToken ct = default)
    {
        var device = RequireDevice();

        lock (_lock)
        {
            if (DateTime.UtcNow - _positionUpdatedAt < TpdoCacheTtl)
                return _cachedPosition;
        }

        var position = await device.ReadInt32Async(ObjPositionActual, _axisSub, ct);

        bool changed;
        int oldPosition;
        lock (_lock)
        {
            oldPosition = _cachedPosition;
            changed = position != _cachedPosition;
            _cachedPosition = position;
        }

        if (changed)
        {
            PositionChanged?.Invoke(this, new PositionChangedEventArgs(position, oldPosition));
            UpdateProperties();
        }

        return position;
    }

    public async Task SetTargetPositionAsync(int position, CancellationToken ct = default)
    {
        lock (_lock)
        {
            _targetPosition = position;
        }

        await RequireDevice().WriteInt32Async(ObjTargetPosition, _axisSub, position, ct);
    }

    public async Task SetProfileSpeedAsync(uint velocity, CancellationToken ct = default)
    {
        // 0x6081 sub theo trục: max speed trong position mode, đơn vị r/min (1-1000)
        await RequireDevice().WriteUInt32Async(ObjProfileVelocity, _axisSub, CountsPerSecToRpm(velocity), ct);
    }

    public async Task SetProfileVelocityAsync(uint velocity, CancellationToken ct = default)
    {
        // Giống driver PhenikaaX: profile velocity ghi vào target velocity (0x60FF)
        await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, CountsPerSecToCommandRaw((int)velocity), ct);
    }

    public async Task SetProfileAccelerationAsync(uint acceleration, CancellationToken ct = default)
    {
        // ZLAC dùng thời gian tăng tốc (ms), không quy đổi được từ counts/s²
        // -> luôn ghi giá trị AccelTimeMs từ config, bỏ qua tham số
        _logger.LogTrace("ZLAC8015D axis {Axis}: SetProfileAcceleration({Value}) -> ghi AccelTimeMs={AccelMs}ms từ config",
            _axis, acceleration, _accelTimeMs);
        await RequireDevice().WriteUInt32Async(ObjProfileAcceleration, _axisSub, _accelTimeMs, ct);
    }

    public async Task SetProfileDecelerationAsync(uint deceleration, CancellationToken ct = default)
    {
        _logger.LogTrace("ZLAC8015D axis {Axis}: SetProfileDeceleration({Value}) -> ghi DecelTimeMs={DecelMs}ms từ config",
            _axis, deceleration, _decelTimeMs);
        await RequireDevice().WriteUInt32Async(ObjProfileDeceleration, _axisSub, _decelTimeMs, ct);
    }

    public async Task<uint> GetProfileSpeedAsync(CancellationToken ct = default)
    {
        return await RequireDevice().ReadUInt32Async(ObjProfileVelocity, _axisSub, ct);
    }

    public async Task<uint> GetProfileAccelerationAsync(CancellationToken ct = default)
    {
        return await RequireDevice().ReadUInt32Async(ObjProfileAcceleration, _axisSub, ct);
    }

    public async Task<uint> GetProfileDecelerationAsync(CancellationToken ct = default)
    {
        return await RequireDevice().ReadUInt32Async(ObjProfileDeceleration, _axisSub, 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 = 1000, uint acceleration = 5000, uint deceleration = 5000, CancellationToken ct = default)
    {
        _logger.LogInformation("Zlac8015dServo[{DeviceId}] axis {Axis}: MoveToPositionAsync position={Position}, velocity={Velocity}",
            DeviceId, _axis, 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);
        await SetTargetPositionAsync(position, ct);
        await Task.Delay(150, ct);
        await StartPositionMoveAsync(ct);
    }

    // Velocity Control (0x60FF sub theo trục)
    /// <summary>
    /// Vận tốc thực tế (counts/s). Ưu tiên cache TPDO khi còn tươi, ngược lại đọc SDO 0x606C:sub.
    /// </summary>
    public async Task<int> GetActualVelocityAsync(CancellationToken ct = default)
    {
        var device = RequireDevice();

        lock (_lock)
        {
            if (DateTime.UtcNow - _velocityUpdatedAt < TpdoCacheTtl)
                return _cachedVelocity;
        }

        var raw = await device.ReadInt32Async(ObjVelocityActual, _axisSub, ct);
        var velocity = FeedbackRawToCountsPerSec(raw);

        bool changed;
        int oldVelocity;
        lock (_lock)
        {
            oldVelocity = _cachedVelocity;
            changed = velocity != _cachedVelocity;
            _cachedVelocity = velocity;
        }

        if (changed)
        {
            VelocityChanged?.Invoke(this, new VelocityChangedEventArgs(velocity, oldVelocity));
            UpdateProperties();
        }

        return velocity;
    }

    public async Task SetTargetVelocityAsync(int velocity, CancellationToken ct = default)
    {
        await RequireDevice().WriteInt32Async(ObjTargetVelocity, _axisSub, CountsPerSecToCommandRaw(velocity), ct);
    }

    /// <summary>
    /// Gửi vận tốc CẢ HAI trục trong MỘT khung RPDO để hai bánh nhận lệnh đồng thời.
    /// Vì 0x60FF:01 và 0x60FF:02 dùng chung một node vật lý, hai lệnh SDO tuần tự sẽ tới
    /// lệch nhau (nặng hơn khi feedback poll đang chiếm kênh SDO), gây hai bánh chạy lệch.
    /// Nếu RPDO không có, fallback ghi SDO THEO LÔ (giữ kênh SDO liên tục cho cả hai lệnh):
    /// độ lệch giữa hai bánh chỉ còn đúng một vòng SDO (~ms), không bị poll feedback chen
    /// vào giữa như khi caller tự ghi hai lệnh SetTargetVelocityAsync riêng lẻ.
    /// Trả false chỉ khi cả hai đường đều thất bại -> caller fallback về SDO tuần tự.
    /// Đơn vị leftCountsPerSec/rightCountsPerSec giống SetTargetVelocityAsync (đã gồm IsReversed).
    /// </summary>
    public async Task<bool> TrySetBothTargetVelocitiesAsync(int leftCountsPerSec, int rightCountsPerSec, CancellationToken ct = default)
    {
        var device = RequireDevice();

        // Conversion counts/s -> raw là như nhau cho cả hai trục (cùng countsPerRevolution
        // và speedResolution), nên tính cả hai từ một instance là chính xác.
        int leftRaw = CountsPerSecToCommandRaw(leftCountsPerSec);
        int rightRaw = CountsPerSecToCommandRaw(rightCountsPerSec);

        if (device.Pdo.IsRpdoConfigured(DualVelocityRpdoNumber))
        {
            var frame = new byte[8];
            BitConverter.TryWriteBytes(frame.AsSpan(0, 4), leftRaw);   // 0x60FF:01
            BitConverter.TryWriteBytes(frame.AsSpan(4, 4), rightRaw);  // 0x60FF:02

            try
            {
                await device.Pdo.SendRPDOAsync(DualVelocityRpdoNumber, frame, ct);
                return true;
            }
            catch (Exception ex)
            {
                _logger.LogWarning(ex, "ZLAC8015D Node {NodeId}: gửi RPDO vận tốc gộp thất bại, thử fallback SDO theo lô", _nodeId);
            }
        }

        try
        {
            await device.WriteInt32BatchAsync(
            [
                (ObjTargetVelocity, (byte)1, leftRaw),
                (ObjTargetVelocity, (byte)2, rightRaw)
            ], ct);
            return true;
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "ZLAC8015D Node {NodeId}: ghi SDO theo lô vận tốc hai trục thất bại", _nodeId);
            return false;
        }
    }

    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 (0x6071 sub theo trục, đơn vị mA)
    public async Task<short> GetActualTorqueAsync(CancellationToken ct = default)
    {
        var value = await RequireDevice().ReadInt16Async(ObjTargetTorque, _axisSub, ct);
        lock (_lock)
        {
            _cachedTorque = value;
            return _cachedTorque;
        }
    }

    public async Task SetTargetTorqueAsync(short torque, CancellationToken ct = default)
    {
        await RequireDevice().WriteInt16Async(ObjTargetTorque, _axisSub, 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);
    }

    // Homing - ZLAC8015D là drive bánh xe hub, không hỗ trợ homing
    public Task SetHomingMethodAsync(byte method, CancellationToken ct = default)
        => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");

    public Task SetHomingSpeedAsync(int speed, CancellationToken ct = default)
        => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");

    public Task SetHomingOffsetAsync(int offset, CancellationToken ct = default)
        => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");

    public Task<byte> GetHomingMethodAsync(CancellationToken ct = default)
        => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");

    public Task<int> GetHomingSpeedAsync(CancellationToken ct = default)
        => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");

    public Task<int> GetHomingOffsetAsync(CancellationToken ct = default)
        => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");

    public Task StartHomingAsync(byte method, int speed, CancellationToken ct = default)
        => throw new NotSupportedException("ZLAC8015D không hỗ trợ homing");

    // 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)
    {
        try
        {
            return await RequireDevice().ReadUInt8Async(0x1001, 0, ct);
        }
        catch
        {
            return 0;
        }
    }

    public async Task<ushort[]> GetErrorHistoryAsync(CancellationToken ct = default)
    {
        try
        {
            var device = RequireDevice();
            byte errorCount = await device.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 device.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

    #region Feedback polling fallback

    /// <summary>
    /// Vòng poll nền làm tươi Position/Velocity qua SDO khi TPDO không về.
    /// Cần thiết vì OdometryService và UI đọc CachedPosition/CachedVelocity một cách đồng bộ,
    /// mà cache đó chỉ do TPDO ghi — TPDO hỏng thì odometry đứng yên mà không báo lỗi gì.
    /// Nếu TPDO hoạt động, TTL còn tươi nên vòng lặp này không phát sinh SDO nào.
    /// </summary>
    private void StartFeedbackPolling()
    {
        StopFeedbackPolling();

        _feedbackPollCts = new CancellationTokenSource();
        var ct = _feedbackPollCts.Token;

        _feedbackPollTask = Task.Run(async () =>
        {
            // Xoay vòng statusword -> position -> velocity, mỗi tick đọc TỐI ĐA MỘT object.
            // Kênh SDO là một hàng đợi tuần tự dùng chung cho cả node (2 trục): nếu mỗi tick
            // đọc cả 3 object x 2 instance thì lệnh điều khiển (controlword, target velocity
            // fallback SDO) phải xếp hàng sau feedback -> điều khiển bị trễ rõ rệt.
            // Với tick 50ms, mỗi giá trị vẫn được làm tươi tối đa mỗi 150ms khi TPDO mất hẳn.
            int pollPhase = 0;

            while (!ct.IsCancellationRequested)
            {
                try
                {
                    await Task.Delay(FeedbackPollIntervalMs, ct);

                    bool statuswordStale, positionStale, velocityStale;
                    lock (_lock)
                    {
                        var now = DateTime.UtcNow;
                        statuswordStale = now - _statuswordUpdatedAt >= TpdoCacheTtl;
                        positionStale = now - _positionUpdatedAt >= TpdoCacheTtl;
                        velocityStale = now - _velocityUpdatedAt >= TpdoCacheTtl;
                    }

                    int selected = -1;
                    for (int i = 0; i < 3; i++)
                    {
                        int idx = (pollPhase + i) % 3;
                        bool isStale = idx switch { 0 => statuswordStale, 1 => positionStale, _ => velocityStale };
                        if (isStale)
                        {
                            selected = idx;
                            pollPhase = idx + 1;
                            break;
                        }
                    }

                    switch (selected)
                    {
                        // Statusword cũng phải được làm tươi, nếu không _cachedStatusword đứng
                        // mãi ở giá trị lần cuối (thường là OperationEnabled sau khi enable xong)
                        // và UI báo OperationEnabled vĩnh viễn kể cả khi drive đã fault.
                        case 0:
                            var before = CachedStatusword.Value;
                            var after = await ReadStatuswordViaSdoAsync(RequireDevice(), ct);
                            if (after.Value != before)
                                UpdateProperties();
                            break;

                        case 1:
                            await GetActualPositionAsync(ct);
                            break;

                        case 2:
                            await GetActualVelocityAsync(ct);
                            break;
                    }

                    // Báo cáo trung thực trạng thái TPDO: chỉ cảnh báo khi TPDO vắng LIÊN TỤC
                    // đủ lâu (không tính giai đoạn khởi động, lúc trục kia còn đang reset/cấu hình
                    // lại node), và báo lại khi TPDO hoạt động trở lại.
                    bool tpdoAlive = !positionStale;

                    if (tpdoAlive)
                    {
                        _tpdoAbsentSinceUtc = null;

                        if (_feedbackPollWarned)
                        {
                            _feedbackPollWarned = false;
                            _logger.LogInformation(
                                "ZLAC8015D Node {NodeId} axis {Axis}: TPDO feedback đã hoạt động trở lại, ngừng fallback SDO.",
                                _nodeId, _axis);
                        }
                    }
                    else
                    {
                        _tpdoAbsentSinceUtc ??= DateTime.UtcNow;

                        if (!_feedbackPollWarned &&
                            DateTime.UtcNow - _tpdoAbsentSinceUtc.Value >= TpdoAbsentGracePeriod)
                        {
                            _feedbackPollWarned = true;
                            _logger.LogWarning(
                                "ZLAC8015D Node {NodeId} axis {Axis}: không nhận được TPDO suốt {Seconds}s, đang fallback poll Statusword/Position/Velocity qua SDO mỗi {IntervalMs}ms. " +
                                "Kiểm tra node đã ở trạng thái Operational chưa.",
                                _nodeId, _axis, TpdoAbsentGracePeriod.TotalSeconds, FeedbackPollIntervalMs);
                        }
                    }
                }
                catch (OperationCanceledException)
                {
                    return;
                }
                catch (Exception ex)
                {
                    _logger.LogDebug(ex, "Feedback poll failed for Node {NodeId} axis {Axis}", _nodeId, _axis);
                }
            }
        }, ct);
    }

    private void StopFeedbackPolling()
    {
        if (_feedbackPollCts == null)
            return;

        try
        {
            _feedbackPollCts.Cancel();
            _feedbackPollCts.Dispose();
        }
        catch
        {
            // Ignore
        }

        _feedbackPollCts = null;
        _feedbackPollTask = null;
    }

    #endregion

    protected override void Dispose(bool disposing)
    {
        if (disposing)
        {
            StopFeedbackPolling();

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