using Appccelerate.StateMachine;
using Appccelerate.StateMachine.Machine;
using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.Shared.Geometry;
using System;
using System.Threading;
using System.Threading.Tasks;

namespace RobotNet10.RobotApp.Motion;

/// <summary>
/// States cho DifferentialDrive state machine - tương tự CiA402 DriveState
/// </summary>
public enum DifferentialDriveState
{
    NotReadyToSwitchOn,
    SwitchOnDisabled,
    ReadyToSwitchOn,
    SwitchedOn,
    OperationEnabled,
    QuickStopActive,
    FaultReactionActive,
    Fault
}

/// <summary>
/// Triggers cho DifferentialDrive state machine - tương tự CiA402 Controlword commands
/// </summary>
public enum DifferentialDriveTrigger
{
    Shutdown,           // Disable voltage
    SwitchOn,           // Switch on command
    DisableVoltage,     // Disable voltage command
    QuickStop,          // Quick stop command
    EnableOperation,    // Enable operation command
    DisableOperation,   // Disable operation command
    FaultReset          // Fault reset command
}

/// <summary>
/// DifferentialDrive service - điều khiển robot di chuyển bằng 2 bánh xe độc lập
/// Implement InverseKinematics và OdometryEstimator
/// Sử dụng state machine để quản lý trạng thái
/// </summary>
public class DifferentialDrive : IInverseKinematics, IOdometryEstimator, IHostedService, IDisposable
{
    private readonly PassiveStateMachine<DifferentialDriveState, DifferentialDriveTrigger> _stateMachine;
    private readonly DifferentialDriveConfiguration _config;
    private readonly IDeviceProvider _deviceProvider;
    private readonly ILogger<DifferentialDrive> _logger;
    private readonly IPlcController? _plcController;
    private readonly Lock _lock = new();

    // Servo devices
    private ICiA402Servo? _leftWheelServo;
    private ICiA402Servo? _rightWheelServo;

    // Odometry state
    private Pose _currentPose = new();
    private int _lastLeftWheelPosition;
    private int _lastRightWheelPosition;
    private bool _isFirstUpdate = true;

    // Calculated parameters
    private double _leftWheelRadius;
    private double _rightWheelRadius;
    public double _wheelbase; // Khoảng cách giữa 2 bánh xe

    // Conversion factors
    private double _leftWheelMetersPerCount; // Mét trên encoder count
    private double _rightWheelMetersPerCount; // Mét trên encoder count

    private DifferentialDriveState _currentState = DifferentialDriveState.SwitchOnDisabled;
    private bool _disposed = false;

    /// <summary>
    /// Gets the current state of the DifferentialDrive
    /// </summary>
    public DifferentialDriveState State => _currentState;

    /// <summary>
    /// Gets whether the drive is in OperationEnabled state (ready to accept velocity commands)
    /// </summary>
    public bool IsOperationEnabled => _currentState == DifferentialDriveState.OperationEnabled;

    /// <summary>
    /// Gets the current pose (odometry)
    /// </summary>
    public Pose CurrentPose
    {
        get
        {
            lock (_lock)
            {
                return _currentPose;
            }
        }
    }

    public DifferentialDrive(
        IConfiguration configuration,
        IDeviceProvider deviceProvider,
        ILogger<DifferentialDrive> logger,
        IPlcController? plcController = null)
    {
        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
        _plcController = plcController;

        // Load configuration
        var configSection = configuration.GetSection("Motion:DifferentialDrive");
        if (!configSection.Exists())
        {
            throw new InvalidOperationException("Configuration section 'Motion:DifferentialDrive' not found in appsettings.json");
        }

        _config = new DifferentialDriveConfiguration();
        configSection.Bind(_config);

        // Validate configuration
        ValidateConfiguration();

        // Calculate parameters
        CalculateParameters();

        // Build state machine
        _stateMachine = BuildStateMachine();
        _stateMachine.Start();
    }

    private void ValidateConfiguration()
    {
        if (string.IsNullOrWhiteSpace(_config.LeftWheel.DeviceId))
            throw new InvalidOperationException("LeftWheel.DeviceId is required");

        if (string.IsNullOrWhiteSpace(_config.RightWheel.DeviceId))
            throw new InvalidOperationException("RightWheel.DeviceId is required");

        if (_config.LeftWheel.WheelDiameter <= 0)
            throw new InvalidOperationException("LeftWheel.WheelDiameter must be greater than 0");

        if (_config.RightWheel.WheelDiameter <= 0)
            throw new InvalidOperationException("RightWheel.WheelDiameter must be greater than 0");

        if (_config.LeftWheel.PulsesPerRevolution <= 0)
            throw new InvalidOperationException("LeftWheel.PulsesPerRevolution must be greater than 0");

        if (_config.RightWheel.PulsesPerRevolution <= 0)
            throw new InvalidOperationException("RightWheel.PulsesPerRevolution must be greater than 0");
    }

    private void CalculateParameters()
    {
        // Calculate wheel radius
        _leftWheelRadius = _config.LeftWheel.WheelDiameter / 2.0;
        _rightWheelRadius = _config.RightWheel.WheelDiameter / 2.0;

        // Calculate meters per encoder count
        var leftWheelCircumference = Math.PI * _config.LeftWheel.WheelDiameter;
        var rightWheelCircumference = Math.PI * _config.RightWheel.WheelDiameter;
        _leftWheelMetersPerCount = leftWheelCircumference / _config.LeftWheel.PulsesPerRevolution;
        _rightWheelMetersPerCount = rightWheelCircumference / _config.RightWheel.PulsesPerRevolution;

        // Extract wheel positions
        var leftPos = _config.LeftWheel.Position.Position;
        var rightPos = _config.RightWheel.Position.Position;

        // Calculate wheelbase (distance between wheel contact points)
        var dx = rightPos.X - leftPos.X;
        var dy = rightPos.Y - leftPos.Y;
        _wheelbase = Math.Sqrt(dx * dx + dy * dy);
        // Console.WriteLine($"rightPos.Y: {rightPos.Y}, leftPos.Y: {leftPos.Y}");

        _logger.LogInformation(
            "DifferentialDrive parameters calculated: LeftRadius={LeftRadius}m, RightRadius={RightRadius}m, " +
            "Wheelbase={Wheelbase}m, LeftY={LeftY}m, RightY={RightY}m",
            _leftWheelRadius, _rightWheelRadius, _wheelbase, leftPos.Y, rightPos.Y);
    }

    private PassiveStateMachine<DifferentialDriveState, DifferentialDriveTrigger> BuildStateMachine()
    {
        var builder = new StateMachineDefinitionBuilder<DifferentialDriveState, DifferentialDriveTrigger>();

        // SwitchOnDisabled state (initial state)
        builder.In(DifferentialDriveState.SwitchOnDisabled)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.SwitchOnDisabled;
                }
                _logger.LogInformation("DifferentialDrive state: SwitchOnDisabled");
            })
            .On(DifferentialDriveTrigger.Shutdown)
                .Goto(DifferentialDriveState.ReadyToSwitchOn)
                .Execute(() =>
                {
                    _ = Task.Run(InitializeAsync);
                });

        // NotReadyToSwitchOn state
        builder.In(DifferentialDriveState.NotReadyToSwitchOn)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.NotReadyToSwitchOn;
                }
                _logger.LogWarning("DifferentialDrive state: NotReadyToSwitchOn");
            })
            .On(DifferentialDriveTrigger.DisableVoltage)
                .Goto(DifferentialDriveState.SwitchOnDisabled);

        // ReadyToSwitchOn state
        builder.In(DifferentialDriveState.ReadyToSwitchOn)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.ReadyToSwitchOn;
                }
                _logger.LogInformation("DifferentialDrive state: ReadyToSwitchOn");
            })
            .On(DifferentialDriveTrigger.SwitchOn)
                .Goto(DifferentialDriveState.SwitchedOn)
                .Execute(() =>
                {
                    _ = Task.Run(SwitchOnAsync);
                })
            .On(DifferentialDriveTrigger.DisableVoltage)
                .Goto(DifferentialDriveState.SwitchOnDisabled)
                .Execute(() =>
                {
                    _ = Task.Run(DisableAsync);
                });

        // SwitchedOn state
        builder.In(DifferentialDriveState.SwitchedOn)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.SwitchedOn;
                }
                _logger.LogInformation("DifferentialDrive state: SwitchedOn");
            })
            .On(DifferentialDriveTrigger.EnableOperation)
                .Goto(DifferentialDriveState.OperationEnabled)
                .Execute(() =>
                {
                    _ = Task.Run(EnableOperationAsync);
                })
            .On(DifferentialDriveTrigger.DisableOperation)
                .Goto(DifferentialDriveState.ReadyToSwitchOn)
            .On(DifferentialDriveTrigger.QuickStop)
                .Goto(DifferentialDriveState.QuickStopActive)
                .Execute(() =>
                {
                    _ = Task.Run(QuickStopAsync);
                })
            .On(DifferentialDriveTrigger.DisableVoltage)
                .Goto(DifferentialDriveState.SwitchOnDisabled)
                .Execute(() =>
                {
                    _ = Task.Run(DisableAsync);
                });

        // OperationEnabled state
        builder.In(DifferentialDriveState.OperationEnabled)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.OperationEnabled;
                }
                _logger.LogInformation("DifferentialDrive state: OperationEnabled");
            })
            .On(DifferentialDriveTrigger.DisableOperation)
                .Goto(DifferentialDriveState.SwitchedOn)
                .Execute(() =>
                {
                    _ = Task.Run(DisableOperationAsync);
                })
            .On(DifferentialDriveTrigger.QuickStop)
                .Goto(DifferentialDriveState.QuickStopActive)
                .Execute(() =>
                {
                    _ = Task.Run(QuickStopAsync);
                })
            .On(DifferentialDriveTrigger.DisableVoltage)
                .Goto(DifferentialDriveState.SwitchOnDisabled)
                .Execute(() =>
                {
                    _ = Task.Run(DisableAsync);
                });

        // QuickStopActive state
        builder.In(DifferentialDriveState.QuickStopActive)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.QuickStopActive;
                }
                _logger.LogWarning("DifferentialDrive state: QuickStopActive");
            })
            .On(DifferentialDriveTrigger.DisableVoltage)
                .Goto(DifferentialDriveState.SwitchOnDisabled)
                .Execute(() =>
                {
                    _ = Task.Run(DisableAsync);
                });

        // FaultReactionActive state
        builder.In(DifferentialDriveState.FaultReactionActive)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.FaultReactionActive;
                }
                _logger.LogError("DifferentialDrive state: FaultReactionActive");
            })
            .On(DifferentialDriveTrigger.FaultReset)
                .Goto(DifferentialDriveState.SwitchOnDisabled)
                .Execute(() =>
                {
                    ResetBothServoFaults();
                    _logger.LogInformation("DifferentialDrive fault reset");
                });

        // Fault state
        builder.In(DifferentialDriveState.Fault)
            .ExecuteOnEntry(() =>
            {
                lock (_lock)
                {
                    _currentState = DifferentialDriveState.Fault;
                }
                _logger.LogError("DifferentialDrive state: Fault");
            })
            .On(DifferentialDriveTrigger.FaultReset)
                .Goto(DifferentialDriveState.SwitchOnDisabled)
                .Execute(() =>
                {
                    ResetBothServoFaults();
                    _logger.LogInformation("DifferentialDrive fault reset");
                });

        return builder
            .WithInitialState(DifferentialDriveState.SwitchOnDisabled)
            .Build()
            .CreatePassiveStateMachine();
    }

    private async Task InitializeAsync()
    {
        try
        {
            // Get servo devices
            var leftDevice = _deviceProvider.GetDevice(_config.LeftWheel.DeviceId);
            var rightDevice = _deviceProvider.GetDevice(_config.RightWheel.DeviceId);

            if (leftDevice is not ICiA402Servo leftServo)
            {
                _logger.LogError("Left wheel device '{DeviceId}' is not an ICiA402Servo", _config.LeftWheel.DeviceId);
                _stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
                return;
            }

            if (rightDevice is not ICiA402Servo rightServo)
            {
                _logger.LogError("Right wheel device '{DeviceId}' is not an ICiA402Servo", _config.RightWheel.DeviceId);
                _stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
                return;
            }

            _leftWheelServo = leftServo;
            _rightWheelServo = rightServo;

            // Subscribe to position changed events
            _leftWheelServo.PositionChanged += OnLeftWheelPositionChanged;
            _rightWheelServo.PositionChanged += OnRightWheelPositionChanged;

            // Subscribe to statusword changed events to detect faults
            _leftWheelServo.StatuswordChanged += OnLeftWheelStatuswordChanged;
            _rightWheelServo.StatuswordChanged += OnRightWheelStatuswordChanged;

            // Set operation mode to Profile Velocity
            await _leftWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity);
            await _rightWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity);

            // Set profile velocity to 0 (đảm bảo động cơ không quay)
            await _leftWheelServo.SetProfileVelocityAsync(0);
            await _rightWheelServo.SetProfileVelocityAsync(0);

            // Set target velocity to 0 (đảm bảo động cơ không quay)
            await _leftWheelServo.SetTargetVelocityAsync(0);
            await _rightWheelServo.SetTargetVelocityAsync(0);

            // Get initial positions
            _lastLeftWheelPosition = _leftWheelServo.CachedPosition;
            _lastRightWheelPosition = _rightWheelServo.CachedPosition;

            _logger.LogInformation("DifferentialDrive initialized successfully - servos ready, velocities set to 0");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Failed to initialize DifferentialDrive");
            _stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
        }
    }

    private async Task DisableAsync()
    {
        try
        {
            // Stop both wheels
            if (_leftWheelServo != null)
            {
                await _leftWheelServo.SetTargetVelocityAsync(0);
                await _leftWheelServo.DisableAsync();
                _leftWheelServo.PositionChanged -= OnLeftWheelPositionChanged;
                _leftWheelServo.StatuswordChanged -= OnLeftWheelStatuswordChanged;
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.SetTargetVelocityAsync(0);
                await _rightWheelServo.DisableAsync();
                _rightWheelServo.PositionChanged -= OnRightWheelPositionChanged;
                _rightWheelServo.StatuswordChanged -= OnRightWheelStatuswordChanged;
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disabling DifferentialDrive");
        }
    }

    private async Task SwitchOnAsync()
    {
        try
        {
            // Switch on both servos (enable voltage, switch on)
            if (_leftWheelServo != null)
            {
                await _leftWheelServo.SwitchOnAsync();
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.SwitchOnAsync();
            }

            _logger.LogInformation("DifferentialDrive switched on");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error switching on DifferentialDrive");
        }
    }

    private async Task EnableOperationAsync()
    {
        try
        {
            // Enable operation on both servos
            if (_leftWheelServo != null)
            {
                await _leftWheelServo.EnableOperationAsync();
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.EnableOperationAsync();
            }

            _logger.LogInformation("DifferentialDrive operation enabled");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error enabling operation");
        }
    }

    private async Task QuickStopAsync()
    {
        try
        {
            // Stop both wheels immediately
            if (_leftWheelServo != null)
            {
                await _leftWheelServo.SetTargetVelocityAsync(0);
                await _leftWheelServo.QuickStopAsync();
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.SetTargetVelocityAsync(0);
                await _rightWheelServo.QuickStopAsync();
            }

            _logger.LogWarning("DifferentialDrive quick stop executed");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error executing quick stop");
        }
    }

    private async Task DisableOperationAsync()
    {
        try
        {
            // Stop both wheels
            if (_leftWheelServo != null)
            {
                await _leftWheelServo.SetTargetVelocityAsync(0);
                await _leftWheelServo.DisableOperationAsync();
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.SetTargetVelocityAsync(0);
                await _rightWheelServo.DisableOperationAsync();
            }

            _logger.LogInformation("DifferentialDrive operation disabled");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disabling operation");
        }
    }

    /// <summary>Reset fault trên cả hai servo CiA402 (trái/phải) trước khi chuyển drive state — gọi khi FaultReset trigger.</summary>
    private void ResetBothServoFaults()
    {
        try
        {
            var left = _leftWheelServo?.FaultResetAsync(CancellationToken.None);
            var right = _rightWheelServo?.FaultResetAsync(CancellationToken.None);
            if (left != null) left.GetAwaiter().GetResult();
            if (right != null) right.GetAwaiter().GetResult();
            _logger.LogInformation("DifferentialDrive: both wheel servos fault reset sent");
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "DifferentialDrive: error resetting servo faults");
        }
    }

    private void OnLeftWheelPositionChanged(object? sender, PositionChangedEventArgs e)
    {
        // Update odometry when either wheel position changes
        UpdateOdometry();
    }

    private void OnRightWheelPositionChanged(object? sender, PositionChangedEventArgs e)
    {
        // Update odometry when either wheel position changes
        UpdateOdometry();
    }

    private void OnLeftWheelStatuswordChanged(object? sender, StatuswordChangedEventArgs e)
    {
        if (e.NewState == DriveState.Fault || e.NewState == DriveState.FaultReactionActive)
        {
            _logger.LogError("Left wheel servo entered fault state: {State}", e.NewState);
            _stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
        }
    }

    private void OnRightWheelStatuswordChanged(object? sender, StatuswordChangedEventArgs e)
    {
        if (e.NewState == DriveState.Fault || e.NewState == DriveState.FaultReactionActive)
        {
            _logger.LogError("Right wheel servo entered fault state: {State}", e.NewState);
            _stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
        }
    }

    #region IInverseKinematics Implementation

    /// <summary>
    /// Tính toán vận tốc các bánh xe từ vận tốc robot (Twist)
    /// Sử dụng position thực tế của bánh xe (không phụ thuộc vào tên "left/right")
    /// 
    /// Công thức kinematics tổng quát cho differential drive:
    /// 1. Tính vận tốc tại contact point: v_contact = v_robot + ω × r_wheel
    /// 2. Bánh xe roll perpendicular to position vector (tangent of circle)
    /// 3. Project v_contact lên roll direction
    /// 4. Apply IsReversed nếu motor đấu ngược
    /// Roll direction = perpendicular to position vector (for differential drive)
    /// </summary>
    public (int leftWheelVelocity, int rightWheelVelocity) CalculateWheelVelocities(Twist twist)
    {
        // Robot velocity
        var vx = twist.Linear.X;
        var vy = twist.Linear.Y;
        var omega = -twist.Angular.Z;
        // var wheelbase = _wheelbase;
        // Console.WriteLine($"vx: {vx}, vy: {vy}, omega: {omega}");
        // Console.WriteLine($"wheelbase: {_wheelbase}");

        // Wheel positions
        var leftPos = _config.LeftWheel.Position.Position;
        var rightPos = _config.RightWheel.Position.Position;

        // Bước 1: Tính vận tốc tại contact point của mỗi bánh xe
        // v_contact = v_robot + ω × r
        // Cross product trong 2D: ω × (x, y) = (-ω×y, ω×x)
        var leftContactVx = vx + omega * leftPos.Y;
        var leftContactVy = vy - omega * leftPos.X;

        var rightContactVx = vx + omega * rightPos.Y;
        var rightContactVy = vy - omega * rightPos.X;
        // Bước 2: Roll direction của bánh xe trong differential drive        
        // Roll direction = perpendicular to position vector (tangent of circle)
        // Với position = (x, y), perpendicular = (-y, x) normalized
        // Nhưng cho differential drive đơn giản với wheels at (0, ±Y):
        // Position vector = (0, Y) → perpendicular = (-Y, 0) normalized = (±1, 0)
        // → Tất cả bánh đều roll theo X direction!
        
        // Roll direction cho differential drive: luôn theo X direction (forward của robot)
        var leftRollDirX = -1.0;
        var leftRollDirY = 0.0;

        var rightRollDirX = -1.0;
        var rightRollDirY = 0.0;

        // Bước 3: Project v_contact lên roll direction (dot product)
        // v_wheel = v_contact · roll_direction
        var leftWheelVelocity = leftContactVx * leftRollDirX + leftContactVy * leftRollDirY;
        var rightWheelVelocity = rightContactVx * rightRollDirX + rightContactVy * rightRollDirY;
        // Console.WriteLine($"leftwheelVelocity (m/s): {leftWheelVelocity}, rightWheelVelocity (m/s): {rightWheelVelocity}");

        // Bước 4: Apply reversed flag if motor/encoder wired backwards
        if (_config.LeftWheel.IsReversed)
            leftWheelVelocity = -leftWheelVelocity;

        if (_config.RightWheel.IsReversed)
            rightWheelVelocity = -rightWheelVelocity;

        // Bước 5: Chuyển đổi từ m/s sang encoder counts/s
        var leftWheelVelocityCounts = (int)(leftWheelVelocity / _leftWheelMetersPerCount);
        var rightWheelVelocityCounts = (int)(rightWheelVelocity / _rightWheelMetersPerCount);
        return (leftWheelVelocityCounts, rightWheelVelocityCounts);
    }

    /// <summary>
    /// Đặt vận tốc robot (Twist) - sẽ tự động tính toán và điều khiển các bánh xe
    /// </summary>
    public async Task SetVelocityAsync(Twist twist, CancellationToken ct = default)
    {
        if (_currentState != DifferentialDriveState.OperationEnabled)
        {
            if (_currentState == DifferentialDriveState.QuickStopActive ||
                _currentState == DifferentialDriveState.SwitchedOn)
            {
                _logger.LogWarning("DifferentialDrive not in OperationEnabled state ({State}), attempting recovery...", _currentState);
                FaultReset();
                await Task.Delay(200, ct);
                Enable();
                await Task.Delay(300, ct);

                if (_currentState != DifferentialDriveState.OperationEnabled)
                {
                    _logger.LogError("Recovery failed: DifferentialDrive not operational. State: {State}", _currentState);
                    return;
                }
            }
            else
            {
                _logger.LogWarning("Cannot set velocity: DifferentialDrive state is {State}", _currentState);
                return;
            }
        }

        var (leftVel, rightVel) = CalculateWheelVelocities(twist);

        try
        {
            // Ưu tiên gửi cả hai bánh trong một khung (RPDO) để chúng nhận lệnh đồng thời.
            // Nếu driver/firmware không hỗ trợ thì fallback về ghi SDO từng bánh.
            bool sentTogether = false;
            if (_leftWheelServo is IDualAxisVelocityCommand dualAxis)
            {
                sentTogether = await dualAxis.TrySetBothTargetVelocitiesAsync(leftVel, rightVel, ct);
            }

            if (!sentTogether)
            {
                if (_leftWheelServo != null)
                {
                    await _leftWheelServo.SetTargetVelocityAsync(leftVel, ct);
                }

                if (_rightWheelServo != null)
                {
                    await _rightWheelServo.SetTargetVelocityAsync(rightVel, ct);
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error setting wheel velocities");
            _stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
        }

        // Truyền hướng di chuyển xuống PLC: tiến M931, lùi M932, không đi thì cả hai off
        var vx = twist.Linear.X;
        const double zeroThreshold = 0.01;
        _plcController?.SetDirectionForwardBackward(vx > zeroThreshold, vx < -zeroThreshold);
    }

    #endregion

    #region IOdometryEstimator Implementation

    /// <summary>
    /// Reset odometry về vị trí ban đầu (0, 0, 0)
    /// </summary>
    public void Reset()
    {
        Reset(new Pose());
    }

    /// <summary>
    /// Reset odometry về một pose cụ thể
    /// </summary>
    public void Reset(Pose pose)
    {
        lock (_lock)
        {
            _currentPose = pose;
            _isFirstUpdate = true;

            if (_leftWheelServo != null)
            {
                _lastLeftWheelPosition = _leftWheelServo.CachedPosition;
            }

            if (_rightWheelServo != null)
            {
                _lastRightWheelPosition = _rightWheelServo.CachedPosition;
            }

            _logger.LogInformation("Odometry reset to pose: X={X}, Y={Y}, Z={Z}", pose.Position.X, pose.Position.Y, pose.Position.Z);
        }
    }

    /// <summary>
    /// Cập nhật odometry từ vị trí encoder của các bánh xe
    /// </summary>
    public void Update(int leftWheelPosition, int rightWheelPosition)
    {
        lock (_lock)
        {
            if (_isFirstUpdate)
            {
                _lastLeftWheelPosition = leftWheelPosition;
                _lastRightWheelPosition = rightWheelPosition;
                _isFirstUpdate = false;
                return;
            }

            // Tính delta position (encoder counts)
            var deltaLeft = leftWheelPosition - _lastLeftWheelPosition;
            var deltaRight = rightWheelPosition - _lastRightWheelPosition;

            // Apply reversed flag (nếu motor đấu ngược, encoder đếm ngược)
            if (_config.LeftWheel.IsReversed)
                deltaLeft = -deltaLeft;

            if (_config.RightWheel.IsReversed)
                deltaRight = -deltaRight;

            // Chuyển đổi sang mét
            var deltaLeftMeters = deltaLeft * _leftWheelMetersPerCount;
            var deltaRightMeters = deltaRight * _rightWheelMetersPerCount;

            // Tính toán vận tốc trung bình và góc quay
            var averageVelocity = (deltaLeftMeters + deltaRightMeters) / 2.0;
            var deltaTheta = (deltaRightMeters - deltaLeftMeters) / _wheelbase;

            // Cập nhật pose
            // Lấy orientation hiện tại từ quaternion
            var currentTheta = QuaternionToYaw(_currentPose.Orientation);

            // Tính toán vị trí mới
            var newX = _currentPose.Position.X + averageVelocity * Math.Cos(currentTheta);
            var newY = _currentPose.Position.Y + averageVelocity * Math.Sin(currentTheta);
            var newTheta = currentTheta + deltaTheta;

            // Cập nhật pose
            _currentPose = new Pose
            {
                Position = new Point(newX, newY, _currentPose.Position.Z),
                Orientation = YawToQuaternion(newTheta)
            };

            // Cập nhật last positions
            _lastLeftWheelPosition = leftWheelPosition;
            _lastRightWheelPosition = rightWheelPosition;
        }
    }

    private void UpdateOdometry()
    {
        if (_leftWheelServo == null || _rightWheelServo == null)
            return;

        Update(_leftWheelServo.CachedPosition, _rightWheelServo.CachedPosition);
    }

    /// <summary>
    /// Chuyển đổi quaternion sang yaw angle (radians)
    /// </summary>
    private static double QuaternionToYaw(Quaternion q)
    {
        // Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
        var sinYaw = 2.0 * (q.W * q.Z + q.X * q.Y);
        var cosYaw = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
        return Math.Atan2(sinYaw, cosYaw);
    }

    /// <summary>
    /// Chuyển đổi yaw angle (radians) sang quaternion
    /// </summary>
    private static Quaternion YawToQuaternion(double yaw)
    {
        var halfYaw = yaw / 2.0;
        return new Quaternion(0, 0, Math.Sin(halfYaw), Math.Cos(halfYaw));
    }

    /// <summary>
    /// Set profile acceleration cho drive system (chuyển đổi từ m/s² sang encoder counts/s²)
    /// </summary>
    /// <param name="acceleration">Gia tốc tăng tốc (m/s²)</param>
    /// <param name="ct">Cancellation token</param>
    public async Task SetAccelerationAsync(double acceleration, CancellationToken ct = default)
    {
        try
        {
            // Chuyển đổi từ m/s² sang encoder counts/s²
            // acceleration_counts = acceleration_ms2 / metersPerCount
            var leftAccelerationCounts = (uint)Math.Abs(acceleration / _leftWheelMetersPerCount);
            var rightAccelerationCounts = (uint)Math.Abs(acceleration / _rightWheelMetersPerCount);

            if (_leftWheelServo != null)
            {
                await _leftWheelServo.SetProfileAccelerationAsync(leftAccelerationCounts, ct);
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.SetProfileAccelerationAsync(rightAccelerationCounts, ct);
            }

            _logger.LogInformation("DifferentialDrive acceleration set to: {Acceleration} m/s² (Left: {LeftCounts}, Right: {RightCounts} counts/s²)", 
                acceleration, leftAccelerationCounts, rightAccelerationCounts);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error setting acceleration to {Acceleration} m/s²", acceleration);
            throw;
        }
    }

    /// <summary>
    /// Set profile deceleration cho drive system (chuyển đổi từ m/s² sang encoder counts/s²)
    /// </summary>
    /// <param name="deceleration">Gia tốc giảm tốc (m/s²)</param>
    /// <param name="ct">Cancellation token</param>
    public async Task SetDecelerationAsync(double deceleration, CancellationToken ct = default)
    {
        try
        {
            // Chuyển đổi từ m/s² sang encoder counts/s²
            var leftDecelerationCounts = (uint)Math.Abs(deceleration / _leftWheelMetersPerCount);
            var rightDecelerationCounts = (uint)Math.Abs(deceleration / _rightWheelMetersPerCount);

            if (_leftWheelServo != null)
            {
                await _leftWheelServo.SetProfileDecelerationAsync(leftDecelerationCounts, ct);
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.SetProfileDecelerationAsync(rightDecelerationCounts, ct);
            }

            _logger.LogInformation("DifferentialDrive deceleration set to: {Deceleration} m/s² (Left: {LeftCounts}, Right: {RightCounts} counts/s²)", 
                deceleration, leftDecelerationCounts, rightDecelerationCounts);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error setting deceleration to {Deceleration} m/s²", deceleration);
            throw;
        }
    }

    #endregion

    #region Public Control Methods

    /// <summary>
    /// Shutdown command - transition from SwitchOnDisabled to ReadyToSwitchOn
    /// </summary>
    public void Shutdown()
    {
        _stateMachine.Fire(DifferentialDriveTrigger.Shutdown);
    }

    /// <summary>
    /// Switch on command - transition from ReadyToSwitchOn to SwitchedOn
    /// </summary>
    public void SwitchOn()
    {
        _stateMachine.Fire(DifferentialDriveTrigger.SwitchOn);
    }

    /// <summary>
    /// Enable operation command - transition from SwitchedOn to OperationEnabled
    /// </summary>
    public void EnableOperation()
    {
        _stateMachine.Fire(DifferentialDriveTrigger.EnableOperation);
    }

    /// <summary>
    /// Disable operation command - transition from OperationEnabled to SwitchedOn
    /// </summary>
    public void DisableOperation()
    {
        _stateMachine.Fire(DifferentialDriveTrigger.DisableOperation);
    }

    /// <summary>
    /// Quick stop command - transition to QuickStopActive
    /// </summary>
    public void QuickStop()
    {
        _stateMachine.Fire(DifferentialDriveTrigger.QuickStop);
    }

    /// <summary>
    /// Disable voltage command - transition to SwitchOnDisabled
    /// </summary>
    public void DisableVoltage()
    {
        _stateMachine.Fire(DifferentialDriveTrigger.DisableVoltage);
    }

    /// <summary>
    /// Fault reset command - luôn gửi fault reset xuống 2 servo, rồi chuyển state machine (nếu đang Fault/FaultReactionActive).
    /// </summary>
    public void FaultReset()
    {
        ResetBothServoFaults();
        _stateMachine.Fire(DifferentialDriveTrigger.FaultReset);
    }

    /// <summary>
    /// Enable DifferentialDrive - convenience method to go through full sequence
    /// </summary>
    public void Enable()
    {
        // Auto-transition through states if needed
        if (_currentState == DifferentialDriveState.SwitchOnDisabled)
        {
            Shutdown();
        }
        else if (_currentState == DifferentialDriveState.ReadyToSwitchOn)
        {
            SwitchOn();
        }
        else if (_currentState == DifferentialDriveState.SwitchedOn)
        {
            EnableOperation();
        }
    }

    /// <summary>
    /// Enable 2 động cơ giống enable bằng tay trên device: gửi lệnh trực tiếp xuống từng servo, await từng bước.
    /// </summary>
    public async Task EnableAsync(CancellationToken cancellationToken = default)
    {
        if (_leftWheelServo == null || _rightWheelServo == null)
        {
            _logger.LogWarning("DifferentialDrive.EnableAsync: servo chưa có, bỏ qua");
            return;
        }
        try
        {
            await _leftWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity, cancellationToken);
            await _rightWheelServo.SetOperationModeAsync(OperationMode.ProfileVelocity, cancellationToken);
            await _leftWheelServo.SetProfileVelocityAsync(0, cancellationToken);
            await _rightWheelServo.SetProfileVelocityAsync(0, cancellationToken);
            await _leftWheelServo.SetTargetVelocityAsync(0, cancellationToken);
            await _rightWheelServo.SetTargetVelocityAsync(0, cancellationToken);
            await _leftWheelServo.SwitchOnAsync(cancellationToken);
            await _rightWheelServo.SwitchOnAsync(cancellationToken);
            await _leftWheelServo.EnableOperationAsync(cancellationToken);
            await _rightWheelServo.EnableOperationAsync(cancellationToken);
            lock (_lock)
            {
                _currentState = DifferentialDriveState.OperationEnabled;
            }
            // Đồng bộ state machine với trạng thái thực để device/điều khiển hoạt động đúng
            _stateMachine.Fire(DifferentialDriveTrigger.Shutdown);
            _stateMachine.Fire(DifferentialDriveTrigger.SwitchOn);
            _stateMachine.Fire(DifferentialDriveTrigger.EnableOperation);
            _logger.LogInformation("DifferentialDrive: 2 động cơ đã enable (EnableAsync), state machine đã sync");
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "DifferentialDrive.EnableAsync: lỗi khi enable động cơ");
        }
    }

    /// <summary>
    /// Disable DifferentialDrive - convenience method
    /// </summary>
    public void Disable()
    {
        if (_currentState == DifferentialDriveState.OperationEnabled)
        {
            DisableOperation();
        }
        else if (_currentState == DifferentialDriveState.SwitchedOn || 
                 _currentState == DifferentialDriveState.ReadyToSwitchOn)
        {
            DisableVoltage();
        }
    }

    /// <summary>
    /// Set operation mode cho cả hai wheel servos
    /// </summary>
    /// <param name="mode">Operation mode to set</param>
    /// <param name="ct">Cancellation token</param>
    public async Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default)
    {
        try
        {
            if (_leftWheelServo != null)
            {
                await _leftWheelServo.SetOperationModeAsync(mode, ct);
            }

            if (_rightWheelServo != null)
            {
                await _rightWheelServo.SetOperationModeAsync(mode, ct);
            }

            _logger.LogInformation("DifferentialDrive operation mode set to: {Mode}", mode);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error setting operation mode to {Mode}", mode);
            throw;
        }
    }

    /// <summary>
    /// Get current operation mode của left wheel servo (giả sử cả 2 bánh có cùng mode)
    /// </summary>
    /// <param name="ct">Cancellation token</param>
    public async Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default)
    {
        try
        {
            if (_leftWheelServo != null)
            {
                return await _leftWheelServo.GetOperationModeAsync(ct);
            }

            _logger.LogWarning("Cannot get operation mode: Left wheel servo not available");
            return OperationMode.NoMode;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error getting operation mode");
            return OperationMode.NoMode;
        }
    }

    #endregion

    #region IHostedService

    /// <summary>
    /// Start DifferentialDrive service — init chạy nền để không chặn Kestrel/web UI lúc startup.
    /// </summary>
    public Task StartAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Starting DifferentialDrive service (non-blocking)...");
        _ = Task.Run(() => InitializeWhenDevicesReadyAsync(cancellationToken), cancellationToken);
        return Task.CompletedTask;
    }

    private async Task InitializeWhenDevicesReadyAsync(CancellationToken cancellationToken)
    {
        try
        {
            _logger.LogInformation("Waiting for all devices to be connected...");
            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);

            if (!connected)
            {
                _logger.LogWarning("Timeout waiting for devices to connect. DifferentialDrive will not be initialized.");
                return;
            }

            _logger.LogInformation("All devices connected. Initializing DifferentialDrive...");
            Shutdown();
        }
        catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
        {
            _logger.LogInformation("DifferentialDrive initialization cancelled");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error starting DifferentialDrive service");
        }
    }

    /// <summary>
    /// Stop DifferentialDrive service
    /// </summary>
    public async Task StopAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Stopping DifferentialDrive service...");

        try
        {
            await DisableOperationAsync();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error stopping DifferentialDrive service");
        }
    }

    #endregion

    #region IDisposable

    public void Dispose()
    {
        if (_disposed)
            return;

        _disposed = true;

        try
        {
            Disable();
            _stateMachine.Stop();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing DifferentialDrive");
        }
    }

    #endregion
}