using System.Runtime.InteropServices;
using System.Text;
using Appccelerate.StateMachine;
using Appccelerate.StateMachine.Machine;
using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Modules;
using RobotNet10.RobotApp.Natives;
using RobotNet10.Shared.Geometry;
// using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.Motion;

/// <summary>
/// States cho ManualControlService state machine
/// State được xác định tự động dựa trên dữ liệu từ RF Handle
/// </summary>
public enum ManualControlState
{
    /// <summary>Initial state, waiting for device connection</summary>
    Initialization,
    /// <summary>No signal from RF Handle (RemoteReady == false)</summary>
    Disabled,
    /// <summary>EStop is pressed (has signal but emergency stop active)</summary>
    SafeStop,
    /// <summary>Has signal + no EStop + mode is Unknown</summary>
    Active,
    /// <summary>Has signal + no EStop + Mode == "Maintenance" - allows robot control</summary>
    Maintenance,
    /// <summary>Has signal + no EStop + Mode == "Override" - allows robot control</summary>
    Override,
    /// <summary>Has signal + no EStop + Mode == "Default" - reserved for future</summary>
    Default
}

/// <summary>
/// Service điều khiển robot từ tay điều khiển RF Handle
/// Chuyển đổi dữ liệu từ IRfHandle thành Twist và điều khiển IInverseKinematics
/// </summary>
public class ManualControlService : IHostedService, IDisposable
{
    private readonly ManualControlConfiguration _config;
    private readonly IServiceProvider _serviceProvider;
    private readonly IDeviceProvider _deviceProvider;
    private readonly IInverseKinematics _inverseKinematics;
    private readonly ILiftModule _liftModule;
    private readonly IRotationModule _rotationModule;
    private readonly IPlcController _plcController;
    private readonly ILogger<ManualControlService> _logger;
    private readonly Lock _lock = new();

    private IRfHandle? _rfHandle;
    private CancellationTokenSource? _updateCts;
    private Thread? _updateThread;
    private ManualControlState _currentState = ManualControlState.Initialization;
    private ManualControlState _previousState = ManualControlState.Initialization;
    private ManualControlState? _externallySetState = null; // State set by RobotStateMachine
    private bool _wasInControlState = false; // Track state transitions for Maintenance/Override
    private bool _disposed = false;
    private volatile bool _isRunning = false; // Thread-safe running state
    private int _updateLoopCounter = 0;

    // ModeSelect button hold tracking
    private DateTime? _modeSelectPressedTime = null;
    private const int ModeSelectHoldDurationMs = 2000; // 2 seconds hold required

    // RF Mode change tracking - notifies RobotController when RF Mode changes
    private RFMode _previousRfMode = RFMode.None;

    /// <summary>
    /// Event raised when RF Handle mode changes.
    /// RobotController subscribes to this to handle state transitions with PLC sync.
    /// </summary>
    public event Action<RFMode>? OnRfModeChanged;

    // Keyboard input state (khi UsingKeyboard = true)
    private Thread? _keyboardThread;
    private readonly Dictionary<ConsoleKey, bool> _keyStates = new();
    private int _keyboardSpeed = 50; // Default 50%
    private bool _keyboardReady = false;

    // Linux evdev keyboard state
    private int _evdevFd = -1;
    private const int INPUT_DEVICE_MAX = 32;  // Max /dev/input/event* devices to check

    // Current velocity being sent to robot
    private Twist _currentTwist = new();

    /// <summary>
    /// Gets the current state of the ManualControlService
    /// </summary>
    public ManualControlState State => _currentState;

    /// <summary>
    /// Gets the current twist being sent to robot
    /// </summary>
    public Twist CurrentTwist
    {
        get
        {
            lock (_lock)
            {
                return _currentTwist;
            }
        }
    }

    /// <summary>
    /// Gets the current RF Handle status (if available)
    /// </summary>
    public RfHandleStatus? CurrentRfHandleStatus
    {
        get
        {
            if (_rfHandle == null)
                return null;

            lock (_lock)
            {
                return new RfHandleStatus
                {
                    Heartbeat = _rfHandle.Heartbeat,
                    Ready = _rfHandle.RemoteReady,
                    EStop = _rfHandle.EStop,
                    Enable = _rfHandle.Enable,
                    Speed = _rfHandle.Speed,
                    Mode = _plcController.CurrentRFMode.ToString(),
                    LastUpdateTime = _rfHandle.LastUpdateTime
                };
            }
        }
    }

    /// <summary>
    /// Maps Linux evdev key codes to ConsoleKey enum
    /// </summary>
    private static readonly Dictionary<ushort, ConsoleKey> LinuxKeyToConsoleKey = new()
    {
        { EvdevNative.KEY_W, ConsoleKey.W },
        { EvdevNative.KEY_A, ConsoleKey.A },
        { EvdevNative.KEY_S, ConsoleKey.S },
        { EvdevNative.KEY_D, ConsoleKey.D },
        { EvdevNative.KEY_Q, ConsoleKey.Q },
        { EvdevNative.KEY_E, ConsoleKey.E },
        { EvdevNative.KEY_Z, ConsoleKey.Z },
        { EvdevNative.KEY_C, ConsoleKey.C },
        { EvdevNative.KEY_SPACE, ConsoleKey.Spacebar },
        { EvdevNative.KEY_MINUS, ConsoleKey.Subtract },
        { EvdevNative.KEY_EQUAL, ConsoleKey.Add },
        { EvdevNative.KEY_KPMINUS, ConsoleKey.Subtract },
        { EvdevNative.KEY_KPPLUS, ConsoleKey.Add },
        { EvdevNative.KEY_ESC, ConsoleKey.Escape }
    };

    public ManualControlService(
        IConfiguration configuration,
        IServiceProvider serviceProvider,
        IDeviceProvider deviceProvider,
        IInverseKinematics inverseKinematics,
        ILiftModule liftModule,
        IRotationModule rotationModule,
        IPlcController plcController,
        ILogger<ManualControlService> logger)
    {
        _serviceProvider = serviceProvider ?? throw new ArgumentNullException(nameof(serviceProvider));
        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));

        // Try to get IInverseKinematics (optional - may not be available)
        _inverseKinematics = inverseKinematics;

        // Try to get ILiftModule (optional - may not be available)
        _liftModule = liftModule;

        // Try to get IRotationModule (optional - may not be available)
        _rotationModule = rotationModule;

        // PLC Controller for system state changes
        _plcController = plcController;

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

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

        // Validate configuration
        ValidateConfiguration();

        // Initialize state
        _currentState = ManualControlState.Initialization;
        _previousState = ManualControlState.Initialization;
    }

    private void ValidateConfiguration()
    {
        if (!_config.Enable) return;

        if (string.IsNullOrWhiteSpace(_config.RfHandleDeviceId))
        {
            if (!_config.UsingKeyboard)
                throw new InvalidOperationException("RfHandleDeviceId is required");
        }

        if (_config.MinLinearVelocity < 0)
            throw new InvalidOperationException("MinLinearVelocity must be >= 0");

        if (_config.MaxLinearVelocity <= _config.MinLinearVelocity)
            throw new InvalidOperationException("MaxLinearVelocity must be > MinLinearVelocity");

        if (_config.MinAngularVelocity < 0)
            throw new InvalidOperationException("MinAngularVelocity must be >= 0");

        if (_config.MaxAngularVelocity <= _config.MinAngularVelocity)
            throw new InvalidOperationException("MaxAngularVelocity must be > MinAngularVelocity");

        if (_config.UpdateRate <= 0)
            throw new InvalidOperationException("UpdateRate must be > 0");

        if (_config.Acceleration <= 0)
            throw new InvalidOperationException("Acceleration must be > 0");

        if (_config.Deceleration <= 0)
            throw new InvalidOperationException("Deceleration must be > 0");
    }

    /// <summary>
    /// Determines the current state based on RF Handle data and externally set state
    /// Called every loop cycle to determine the appropriate state
    ///
    /// State priority:
    /// 1. If RF Handle not connected -> Initialization
    /// 2. If RemoteReady == false -> Disabled (resets external state)
    /// 3. If EStop pressed -> SafeStop (resets external state)
    /// 4. If external state set (Maintenance/Override) -> use external state
    /// 5. Otherwise -> Active (waiting for RobotStateMachine to set state)
    /// </summary>
    private ManualControlState DetermineStateFromRfHandle()
    {
        if (_rfHandle == null)
            return ManualControlState.Initialization;

        // No signal from RF Handle - reset external state
        if (!_rfHandle.RemoteReady)
        {
            _externallySetState = null;
            return ManualControlState.Disabled;
        }

        // Has signal + EStop pressed - keep external state for recovery when EStop released
        if (_rfHandle.EStop)
        {
            _plcController.SetRFEStop(true); // Ensure PLC knows EStop is active
            return ManualControlState.SafeStop;
        }
        else _plcController.SetRFEStop(false);

        // If external state is set (by RobotStateMachine), use it
        if (_externallySetState.HasValue)
        {
            return _externallySetState.Value;
        }

        // Has signal + No EStop + No external state -> Active
        // Note: We do NOT auto-switch to Maintenance/Override based on RF Handle Mode
        // State transitions to Maintenance/Override are controlled by RobotStateMachine
        return ManualControlState.Active;
    }

    /// <summary>
    /// Checks if the current state allows robot control actions
    /// </summary>
    private static bool IsControlAllowedState(ManualControlState state)
        => state == ManualControlState.Maintenance || state == ManualControlState.Override;

    /// <summary>
    /// Called when entering Maintenance or Override state
    /// Sets acceleration/deceleration for IInverseKinematics
    /// </summary>
    private void OnEnterControlState()
    {
        try
        {
            // SetAcceleration/SetDeceleration not available - commented out
            // _inverseKinematics.SetAcceleration(_config.Acceleration);
            // _inverseKinematics.SetDeceleration(_config.Deceleration);
            _logger.LogInformation("Entered control state, Accel={Accel}, Decel={Decel}",
                _config.Acceleration, _config.Deceleration);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error setting acceleration/deceleration on entering control state");
        }
    }

    /// <summary>
    /// Called when exiting Maintenance or Override state
    /// Stops the robot
    /// </summary>
    private void OnExitControlState()
    {
        StopRobot();
        _logger.LogInformation("Exited control state, robot stopped");
    }

    /// <summary>
    /// Handles ModeSelect button hold detection for PLC system state changes
    /// - In Active state: Hold 2 seconds → PlcController.SetSystemState(MAINTENANCE)
    /// - In Maintenance state: Hold 2 seconds → PlcController.SetSystemState(OVERRIDE)
    ///
    /// Note: After PLC changes mode, RF Handle will read new mode, then HandleRfModeChange() fires RobotStateMachine
    /// </summary>
    private void HandleModeSelectButtonHold()
    {
        if (_rfHandle == null || !_plcController.IsReady)
            return;

        bool modeSelectPressed = _rfHandle.ModeSelect;

        if (modeSelectPressed)
        {
            // Button is pressed
            if (_modeSelectPressedTime == null)
            {
                // Start tracking press time
                _modeSelectPressedTime = DateTime.UtcNow;
                _logger.LogDebug("ModeSelect button pressed, starting hold timer");
            }
            else
            {
                // Check if held long enough (2 seconds)
                var holdDuration = (DateTime.UtcNow - _modeSelectPressedTime.Value).TotalMilliseconds;

                if (holdDuration >= ModeSelectHoldDurationMs)
                {
                    // Determine action based on current state
                    switch (_currentState)
                    {
                        case ManualControlState.Active:
                            // Active → SetSystemState(MAINTENANCE) → PLC changes → RF reads new mode → Fire RobotStateMachine
                            _logger.LogInformation("ModeSelect held for 2s in Active state, setting PLC to MAINTENANCE");
                            try
                            {
                                _plcController.SetRFMode(RFMode.Maintenance);
                            }
                            catch (Exception ex)
                            {
                                _logger.LogError(ex, "Error setting system state to MAINTENANCE");
                            }
                            break;

                        case ManualControlState.Maintenance:
                            // Maintenance → SetSystemState(OVERRIDE) → PLC changes → RF reads new mode → Fire RobotStateMachine
                            _logger.LogInformation("ModeSelect held for 2s in Maintenance state, setting PLC to OVERRIDE");
                            try
                            {
                                _plcController.SetRFMode(RFMode.Override);
                            }
                            catch (Exception ex)
                            {
                                _logger.LogError(ex, "Error setting system state to OVERRIDE");
                            }
                            break;

                        default:
                            // Other states - no action
                            break;
                    }

                    // Reset timer to prevent repeated triggers
                    _modeSelectPressedTime = null;
                }
            }
        }
        else
        {
            // Button released - reset timer
            if (_modeSelectPressedTime != null)
            {
                _logger.LogDebug("ModeSelect button released before 2s hold");
                _modeSelectPressedTime = null;
            }
        }
    }

    /// <summary>
    /// Handles RF Mode change detection and notifies RobotController via event
    /// Called every loop cycle after reading RF Handle data
    /// Reads RFMode from PlcController (not from RF Handle device, which doesn't expose Mode)
    ///
    /// Flow: RF Mode changes on PLC → detected here → OnRfModeChanged event → RobotController handles state transition
    /// </summary>
    private void HandleRfModeChange()
    {
        if (_plcController == null || !_plcController.IsReady)
            return;

        var currentRfMode = _plcController.CurrentRFMode;

        // Check if mode changed
        if (currentRfMode != _previousRfMode)
        {
            _logger.LogInformation("RF Mode changed from {Previous} to {Current}", _previousRfMode, currentRfMode);

            // Notify RobotController via event (RobotController handles Pause/Resume + state transitions)
            try
            {
                OnRfModeChanged?.Invoke(currentRfMode);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in OnRfModeChanged handler");
            }

            // Update previous mode
            _previousRfMode = currentRfMode;
        }
    }

    /// <summary>
    /// Start high-priority update thread for real-time velocity control
    /// </summary>
    private void StartUpdateLoop()
    {
        StopUpdateLoop(); // Stop existing loop if any

        // Check if input device is available
        if (_config.UsingKeyboard)
        {
            if (!_keyboardReady)
            {
                _logger.LogWarning("Cannot start update loop: Keyboard input not ready");
                return;
            }
        }
        else
        {
            if (_rfHandle == null)
            {
                _logger.LogWarning("Cannot start update loop: RF Handle not available");
                return;
            }
        }

        lock (_lock)
        {
            _updateCts = new CancellationTokenSource();
            var token = _updateCts.Token; // Store token in local variable to avoid race condition

            if (_config.UsingKeyboard)
            {
                _updateThread = new Thread(() => UpdateFromKeyboardLoop(token))
                {
                    Name = "ManualControl-Update-Keyboard",
                    IsBackground = false, // Không phải background thread để đảm bảo chạy liên tục
                    Priority = ThreadPriority.Highest // Priority cao để đảm bảo real-time control
                };
            }
            else
            {
                _updateThread = new Thread(() => UpdateFromDeviceLoop(token))
                {
                    Name = $"ManualControl-Update-{_config.RfHandleDeviceId}",
                    IsBackground = false, // Không phải background thread để đảm bảo chạy liên tục
                    Priority = ThreadPriority.Highest // Priority cao để đảm bảo real-time control
                };
            }

            _updateThread.Start();
            _logger.LogDebug("ManualControlService: Started high-priority update thread at {UpdateRate}Hz", _config.UpdateRate);
        }
    }

    /// <summary>
    /// Stop update thread gracefully
    /// </summary>
    private void StopUpdateLoop()
    {
        CancellationTokenSource? ctsToCancel;
        Thread? threadToWait;
        lock (_lock)
        {
            ctsToCancel = _updateCts;
            threadToWait = _updateThread;
            _updateCts = null;
            _updateThread = null;
        }

        // Cancel and wait outside the lock to avoid blocking
        if (ctsToCancel != null)
        {
            try
            {
                ctsToCancel.Cancel();
            }
            catch (ObjectDisposedException)
            {
                // CTS already disposed, ignore
            }
        }

        if (threadToWait != null)
        {
            try
            {
                // Wait for thread to finish gracefully
                if (!threadToWait.Join(TimeSpan.FromSeconds(3)))
                {
                    _logger.LogWarning("Timeout waiting for update thread to stop");
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error stopping update thread");
            }
        }

        // Dispose CTS after thread is done
        ctsToCancel?.Dispose();
    }

    /// <summary>
    /// High-priority synchronous update loop for keyboard input - runs at configured UpdateRate
    /// Similar to UpdateFromDeviceLoop but reads from keyboard instead of RF Handle
    /// </summary>
    private void UpdateFromKeyboardLoop(CancellationToken cancellationToken)
    {
        var updateIntervalMs = (int)(1000.0 / _config.UpdateRate);
        var spinWait = new SpinWait();

        Thread.BeginThreadAffinity();
        try
        {
            // Ensure IInverseKinematics is in OperationEnabled state and ProfileVelocity mode
            while (!EnsureInverseKinematicsReady(cancellationToken))
            {
                // IInverseKinematics is not ready, stop robot and continue (will retry next cycle)
                StopRobot();
                PreciseDelay(updateIntervalMs, cancellationToken);
                // Use synchronous delay instead of async Task.Delay to avoid thread pool starvation
                PreciseDelay(500, cancellationToken);
            }

            while (!cancellationToken.IsCancellationRequested)
            {
                var startTime = DateTime.UtcNow;

                try
                {
                    // Increment update loop counter
                    _updateLoopCounter++;

                    // Check if keyboard is ready
                    if (!_keyboardReady)
                    {
                        // Log warning every 10 cycles to avoid spam
                        if (_updateLoopCounter % 10 == 0)
                        {
                            _logger.LogWarning("Cannot send velocity: Keyboard input not ready");
                        }

                        // Stop robot but continue loop
                        StopRobot();
                        PreciseDelay(updateIntervalMs, cancellationToken);
                        continue;
                    }

                    // Check if Stop key is pressed (Space)
                    bool stopPressed = false;
                    lock (_lock)
                    {
                        stopPressed = _keyStates.GetValueOrDefault(ConsoleKey.Spacebar, false);
                    }

                    if (stopPressed)
                    {
                        StopRobot();
                        PreciseDelay(updateIntervalMs, cancellationToken);
                        continue;
                    }

                    // Update velocity from keyboard
                    UpdateVelocityFromKeyboard(cancellationToken);

                    // Handle lift module control
                    HandleLiftModuleControlFromKeyboard(cancellationToken);

                    // Handle rotation module control
                    HandleRotationModuleControlFromKeyboard(cancellationToken);

                    // Calculate elapsed time and sleep for remaining interval
                    var elapsedMs = (DateTime.UtcNow - startTime).TotalMilliseconds;
                    var remainingMs = updateIntervalMs - elapsedMs;

                    if (remainingMs > 0)
                    {
                        PreciseDelay((int)remainingMs, cancellationToken);
                    }
                    else
                    {
                        // If update took longer than interval, log warning and continue immediately
                        if (elapsedMs > updateIntervalMs * 1.5) // Only warn if significantly over
                        {
                            _logger.LogWarning(
                                "ManualControlService: Update loop took {ElapsedMs:F1}ms (exceeds {IntervalMs}ms interval)",
                                elapsedMs, updateIntervalMs);
                        }

                        // Use minimal delay to prevent CPU spinning
                        spinWait.Reset();
                        for (int i = 0; i < 10; i++)
                        {
                            if (cancellationToken.IsCancellationRequested)
                                break;
                            spinWait.SpinOnce();
                        }
                    }
                }
                catch (OperationCanceledException)
                {
                    // Loop cancelled, exit gracefully
                    break;
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "Error in keyboard update loop");

                    // Stop robot on error but continue loop
                    StopRobot();
                    PreciseDelay(updateIntervalMs, cancellationToken);
                }
            }
        }
        finally
        {
            Thread.EndThreadAffinity();
        }
    }

    /// <summary>
    /// High-priority synchronous update loop - runs at configured UpdateRate
    /// This method runs directly in a high-priority thread for optimal real-time performance
    /// State is determined each cycle based on RF Handle data
    /// Control actions only execute in Maintenance or Override states
    /// </summary>
    private void UpdateFromDeviceLoop(CancellationToken cancellationToken)
    {
        var updateIntervalMs = (int)(1000.0 / _config.UpdateRate);
        var spinWait = new SpinWait();

        Thread.BeginThreadAffinity();
        try
        {
            while (!cancellationToken.IsCancellationRequested)
            {
                var startTime = DateTime.UtcNow;

                try
                {
                    // Increment update loop counter
                    _updateLoopCounter++;

                    // STEP 1: Determine current state from RF Handle data
                    var newState = DetermineStateFromRfHandle();

                    // STEP 2: Handle state transitions
                    bool isInControlState = IsControlAllowedState(newState);
                    bool stateChanged = newState != _currentState;

                    if (isInControlState && !_wasInControlState)
                    {
                        // Entering control state (Maintenance or Override)
                        _logger.LogInformation("Entering control state: {State}", newState);
                        OnEnterControlState();

                        // Ensure IK is ready when entering control state
                        if (!EnsureInverseKinematicsReady(cancellationToken))
                        {
                            StopRobot();
                            PreciseDelay(updateIntervalMs, cancellationToken);
                            continue;
                        }
                    }
                    else if (!isInControlState && _wasInControlState)
                    {
                        // Exiting control state to non-control state
                        _logger.LogInformation("Exiting control state, new state: {State}", newState);
                        OnExitControlState(); // This calls StopRobot() once
                    }
                    else if (!isInControlState && stateChanged)
                    {
                        // Transitioning between non-control states (e.g., Disabled → SafeStop)
                        // Ensure robot is stopped when changing states
                        _logger.LogInformation("State changed to: {State}", newState);
                        StopRobot();
                    }

                    // STEP 3: Update state tracking
                    lock (_lock)
                    {
                        _previousState = _currentState;
                        _currentState = newState;
                    }
                    _wasInControlState = isInControlState;

                    // STEP 3.4: Detect RF Handle disconnection → notify RobotController to release RF priority
                    if (newState == ManualControlState.Disabled && stateChanged
                        && _previousState != ManualControlState.Initialization)
                    {
                        // Clear PLC RF Mode to prevent HandleRfModeChange() from re-detecting old mode
                        try { _plcController.SetRFMode(RFMode.None); }
                        catch (Exception ex) { _logger.LogError(ex, "Error clearing PLC RF mode on disconnect"); }

                        try
                        {
                            OnRfModeChanged?.Invoke(RFMode.None);
                        }
                        catch (Exception ex)
                        {
                            _logger.LogError(ex, "Error in OnRfModeChanged handler (disconnect)");
                        }
                        _previousRfMode = RFMode.None;
                    }

                    // STEP 3.5: Handle ModeSelect button hold for PLC system state changes
                    // - Active + ModeSelect held 2s → PlcController.SetSystemState(MAINTENANCE)
                    // - Maintenance + ModeSelect held 2s → PlcController.SetSystemState(OVERRIDE)
                    HandleModeSelectButtonHold();

                    // STEP 3.6: Handle RF Mode change - Fire RobotStateMachine when RF Mode changes
                    // This happens after PLC changes mode and RF Handle reads new mode
                    HandleRfModeChange();

                    // STEP 4: Execute actions based on state
                    if (isInControlState)
                    {
                        // In Maintenance or Override: Execute full control
                        // Ensure IK is ready (may have been disabled during operation)
                        if (!EnsureInverseKinematicsReady(cancellationToken))
                        {
                            StopRobot();
                            PreciseDelay(updateIntervalMs, cancellationToken);
                            continue;
                        }

                        UpdateVelocityFromRfHandle(cancellationToken);
                        HandleLiftModuleControl(cancellationToken);
                        HandleRotationModuleControl(cancellationToken);
                    }
                    else
                    {
                        // In non-control states (Disabled, SafeStop, Active without external state, etc.)
                        // StopRobot() is already called ONCE in OnExitControlState() when transitioning
                        // No need to call it repeatedly every cycle

                        // Log state periodically (every 100 cycles = ~5 seconds at 20Hz)
                        if (_updateLoopCounter % 100 == 0)
                        {
                            _logger.LogDebug("ManualControl state: {State}", newState);
                        }
                    }

                    // STEP 5: Timing control
                    var elapsedMs = (DateTime.UtcNow - startTime).TotalMilliseconds;
                    var remainingMs = updateIntervalMs - elapsedMs;

                    if (remainingMs > 0)
                    {
                        PreciseDelay((int)remainingMs, cancellationToken);
                    }
                    else
                    {
                        // If update took longer than interval, log warning and continue immediately
                        if (elapsedMs > updateIntervalMs * 1.5) // Only warn if significantly over
                        {
                            _logger.LogWarning(
                                "ManualControlService: Update loop took {ElapsedMs:F1}ms (exceeds {IntervalMs}ms interval)",
                                elapsedMs, updateIntervalMs);
                        }

                        // Use minimal delay to prevent CPU spinning
                        spinWait.Reset();
                        for (int i = 0; i < 10; i++)
                        {
                            if (cancellationToken.IsCancellationRequested)
                                break;
                            spinWait.SpinOnce();
                        }
                    }
                }
                catch (OperationCanceledException)
                {
                    // Loop cancelled, exit gracefully
                    break;
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "Error in update loop");

                    // Stop robot on error but continue loop
                    StopRobot();
                    PreciseDelay(updateIntervalMs, cancellationToken);
                }
            }
        }
        finally
        {
            Thread.EndThreadAffinity();
            // Always stop robot when loop exits
            StopRobot();
        }
    }

    /// <summary>
    /// Precise synchronous delay using Thread.Sleep for longer delays and SpinWait for short delays
    /// This ensures accurate timing for the update loop without async overhead
    /// </summary>
    private static void PreciseDelay(int milliseconds, CancellationToken cancellationToken)
    {
        if (milliseconds <= 0)
            return;

        if (milliseconds > 1)
        {
            // Use Thread.Sleep for longer delays (synchronous, more precise in dedicated thread)
            // Check cancellation periodically during sleep
            var sleepStart = DateTime.UtcNow;
            while ((DateTime.UtcNow - sleepStart).TotalMilliseconds < milliseconds)
            {
                if (cancellationToken.IsCancellationRequested)
                    return;

                var remaining = milliseconds - (int)(DateTime.UtcNow - sleepStart).TotalMilliseconds;
                if (remaining > 0)
                {
                    Thread.Sleep(Math.Min(remaining, 10)); // Sleep in 10ms chunks to check cancellation
                }
            }
        }
        else
        {
            // Use SpinWait for very short delays to maintain precise timing
            var spinWait = new SpinWait();
            for (int i = 0; i < 10; i++)
            {
                if (cancellationToken.IsCancellationRequested)
                    break;
                spinWait.SpinOnce();
            }
        }
    }

    /// <summary>
    /// Synchronous version of UpdateVelocityFromRfHandleAsync for use in high-priority thread
    /// Uses ConfigureAwait(false) and timeout to prevent blocking when CPU load is high
    /// </summary>
    private void UpdateVelocityFromRfHandle(CancellationToken cancellationToken)
    {
        if (_rfHandle == null)
            return;

        // Read joy state to update RF Handle internal state (even though we use button properties directly)
        // This ensures properties like Forward, Backward, Left, Right, Speed are up-to-date
        var joyState = _rfHandle.CurrentJoyState;
        if (joyState == null)
        {
            // Try to read if not cached with timeout to prevent blocking
            // Direct async call with timeout since we're in a dedicated high-priority thread
            try
            {
                var readTask = _rfHandle.ReadJoyStateAsync(cancellationToken);

                if (!readTask.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout reading joy state (50ms)");
                    StopRobot();
                    return;
                }
            }
            catch (Exception ex)
            {
                _logger.LogWarning(ex, "Failed to read joy state");
                StopRobot();
                return;
            }
        }

        // Calculate velocity from RF Handle buttons (Forward, Backward, Left, Right)
        var twist = CalculateTwistFromRfHandle();

        // Update current twist (minimal lock time)
        lock (_lock)
        {
            _currentTwist = twist;
        }

        // Send to IInverseKinematics (synchronous)
        // Note: IInverseKinematics state is already ensured in UpdateLoop
        try
        {
            var task = _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
            if (!task.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
            {
                _logger.LogWarning("Timeout sending velocity to IInverseKinematics (50ms) - CPU may be overloaded");
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error sending velocity to IInverseKinematics");
        }
    }

    /// <summary>
    /// Synchronous version of EnsureInverseKinematicsReadyAsync for use in high-priority thread
    /// Đảm bảo IInverseKinematics ở trạng thái OperationEnabled và mode ProfileVelocity
    /// Tự động enable nếu cần, reset fault nếu có
    /// Uses ConfigureAwait(false) and timeout to prevent blocking when CPU load is high
    /// </summary>
    /// <returns>True nếu IInverseKinematics ready, False nếu không</returns>
    private bool EnsureInverseKinematicsReady(CancellationToken cancellationToken)
    {
        try
        {
            // Check if need to reset fault first
            // Note: DifferentialDrive doesn't expose IsFaulted, so we try FaultReset if not enabled
            if (!_inverseKinematics.IsOperationEnabled)
            {
                // Try fault reset first (in case it's in fault state)
                _inverseKinematics.FaultReset();
                PreciseDelay(200, cancellationToken);
            }

            // Check if IInverseKinematics is in OperationEnabled state
            if (!_inverseKinematics.IsOperationEnabled)
            {
                // Auto-enable IInverseKinematics through state transitions
                // Enable() is a convenience method that automatically transitions through all states
                int maxAttempts = 3; // Reduced attempts since this runs in loop
                int attemptDelay = 300; // ms

                for (int i = 0; i < maxAttempts && !_inverseKinematics.IsOperationEnabled; i++)
                {
                    _inverseKinematics.Enable();
                    PreciseDelay(attemptDelay, cancellationToken);
                }

                // Check if enabled successfully
                if (!_inverseKinematics.IsOperationEnabled)
                {
                    // Log only once every 10 times to avoid spam
                    if (_updateLoopCounter % 10 == 0)
                    {
                        _logger.LogWarning("IInverseKinematics is not in OperationEnabled state. Cannot send velocity.");
                    }
                    return false;
                }
            }

            // Check and set operation mode to ProfileVelocity (synchronous)
            try
            {
                var modeTask = _inverseKinematics.GetOperationModeAsync(cancellationToken);
                if (!modeTask.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout getting operation mode (50ms) - CPU may be overloaded");
                    return true; // Continue anyway
                }

                OperationMode currentMode = modeTask.Result;

                if (currentMode != OperationMode.ProfileVelocity)
                {
                    var setTask = _inverseKinematics.SetOperationModeAsync(OperationMode.ProfileVelocity, cancellationToken);
                    if (!setTask.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
                    {
                        _logger.LogWarning("Timeout setting operation mode (50ms) - CPU may be overloaded");
                        return true; // Continue anyway
                    }
                }
            }
            catch (Exception ex)
            {
                // Log only once every 10 times to avoid spam
                if (_updateLoopCounter % 10 == 0)
                {
                    _logger.LogWarning(ex, "Error checking/setting operation mode");
                }
                // Continue anyway to prevent blocking
            }

            return true;
        }
        catch (OperationCanceledException)
        {
            throw;
        }
        catch (Exception ex)
        {
            // Log only once every 10 times to avoid spam
            if (_updateLoopCounter % 10 == 0)
            {
                _logger.LogError(ex, "Error ensuring IInverseKinematics ready");
            }
            return false;
        }
    }

    private Twist CalculateTwistFromRfHandle()
    {
        var twist = new Twist();

        // Get speed percentage (0-100)
        var speedPercent = _rfHandle?.Speed ?? 0;
        if (speedPercent < 0) speedPercent = 0;
        if (speedPercent > 100) speedPercent = 100;

        // Calculate velocity range based on speed percentage
        var linearVelocityRange = _config.MaxLinearVelocity - _config.MinLinearVelocity;
        var angularVelocityRange = _config.MaxAngularVelocity - _config.MinAngularVelocity;
        var speedFactor = speedPercent / 100.0;

        // Use buttons from RF Handle: Forward/Backward for linear, Left/Right for angular
        double linearInput = _rfHandle?.Linear ?? 0;
        double angularInput = _rfHandle?.Angular ?? 0;

        // Apply deadzone (min velocity threshold)
        var linearDeadzone = _config.MinLinearVelocity / _config.MaxLinearVelocity;
        var angularDeadzone = _config.MinAngularVelocity / _config.MaxAngularVelocity;

        if (Math.Abs(linearInput) < linearDeadzone)
            linearInput = 0.0;
        else
        {
            // Normalize after deadzone
            var sign = Math.Sign(linearInput);
            linearInput = (sign * ((Math.Abs(linearInput) - linearDeadzone) / (1.0 - linearDeadzone)));
        }

        if (Math.Abs(angularInput) < angularDeadzone)
            angularInput = 0.0;
        else
        {
            // Normalize after deadzone
            var sign = Math.Sign(angularInput);
            angularInput = (sign * ((Math.Abs(angularInput) - angularDeadzone) / (1.0 - angularDeadzone)));
        }

        // Calculate final velocities
        var linearVelocity = _config.MinLinearVelocity + linearInput * linearVelocityRange * speedFactor;
        var angularVelocity = _config.MinAngularVelocity + angularInput * angularVelocityRange * speedFactor;

        twist.Linear = new Vector3(linearVelocity, 0, 0);
        twist.Angular = new Vector3(0, 0, angularVelocity);

        return twist;
    }

    /// <summary>
    /// Synchronous version of HandleLiftModuleControlAsync for use in high-priority thread
    /// Uses timeout to prevent blocking when CPU load is high
    /// </summary>
    private void HandleLiftModuleControl(CancellationToken cancellationToken)
    {
        // Check if lift module is ready
        if (_rfHandle == null || _liftModule.State != LiftModuleState.Ready)
            return;

        try
        {
            // Handle lift up
            if (_rfHandle.LiftUp)
            {
                var task = _liftModule.LiftUpAsync(cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling lift up (200ms) - CPU may be overloaded");
                }
            }
            // Handle lift down
            else if (_rfHandle.LiftDown)
            {
                var task = _liftModule.LiftDownAsync(cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling lift down (200ms) - CPU may be overloaded");
                }
            }
        }
        catch (OperationCanceledException)
        {
            throw;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error handling lift module control");
        }
    }

    /// <summary>
    /// Synchronous version of HandleRotationModuleControlAsync for use in high-priority thread
    /// Uses timeout to prevent blocking when CPU load is high
    /// </summary>
    private void HandleRotationModuleControl(CancellationToken cancellationToken)
    {
        // Check if rotation module is ready
        if (_rfHandle == null || _rotationModule.State != RotationModuleState.Ready)
            return;

        try
        {
            // Handle rotate left (90 degrees)
            if (_rfHandle.RotateLeft)
            {
                var task = _rotationModule.RotateOffsetAsync(90.0, cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling rotate left (200ms) - CPU may be overloaded");
                }
            }
            // Handle rotate right (-90 degrees)
            else if (_rfHandle.RotateRight)
            {
                var task = _rotationModule.RotateOffsetAsync(-90.0, cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling rotate right (200ms) - CPU may be overloaded");
                }
            }
        }
        catch (OperationCanceledException)
        {
            throw;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error handling rotation module control");
        }
    }

    /// <summary>
    /// Update velocity from keyboard input
    /// </summary>
    private void UpdateVelocityFromKeyboard(CancellationToken cancellationToken)
    {
        if (!_keyboardReady)
            return;

        // Calculate velocity from keyboard
        var twist = CalculateTwistFromKeyboard();

        // Update current twist (minimal lock time)
        lock (_lock)
        {
            _currentTwist = twist;
        }

        // Send to IInverseKinematics (synchronous)
        try
        {
            var task = _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
            if (!task.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
            {
                _logger.LogWarning("Timeout sending velocity to IInverseKinematics (50ms) - CPU may be overloaded");
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error sending velocity to IInverseKinematics");
        }
    }

    /// <summary>
    /// Calculate Twist from keyboard input
    /// W/S: Linear forward/backward
    /// A/D: Angular left/right
    /// </summary>
    private Twist CalculateTwistFromKeyboard()
    {
        var twist = new Twist();

        // Get key states
        bool forward, backward, left, right;
        int speedPercent;

        lock (_lock)
        {
            forward = _keyStates.GetValueOrDefault(ConsoleKey.W, false);
            backward = _keyStates.GetValueOrDefault(ConsoleKey.S, false);
            left = _keyStates.GetValueOrDefault(ConsoleKey.A, false);
            right = _keyStates.GetValueOrDefault(ConsoleKey.D, false);
            speedPercent = _keyboardSpeed;
        }

        // Calculate linear input (-1.0 to 1.0)
        double linearInput = 0.0;
        if (forward && !backward) linearInput = 1.0;
        else if (backward && !forward) linearInput = -1.0;

        // Calculate angular input (-1.0 to 1.0)
        double angularInput = 0.0;
        if (right && !left) angularInput = 1.0;
        else if (left && !right) angularInput = -1.0;

        // Get speed percentage (0-100)
        if (speedPercent < 0) speedPercent = 0;
        if (speedPercent > 100) speedPercent = 100;

        // Calculate velocity range based on speed percentage
        var linearVelocityRange = _config.MaxLinearVelocity - _config.MinLinearVelocity;
        var angularVelocityRange = _config.MaxAngularVelocity - _config.MinAngularVelocity;
        var speedFactor = speedPercent / 100.0;

        // Apply deadzone (min velocity threshold)
        var linearDeadzone = _config.MinLinearVelocity / _config.MaxLinearVelocity;
        var angularDeadzone = _config.MinAngularVelocity / _config.MaxAngularVelocity;

        if (Math.Abs(linearInput) < linearDeadzone)
            linearInput = 0.0;
        else
        {
            // Normalize after deadzone
            var sign = Math.Sign(linearInput);
            linearInput = (sign * ((Math.Abs(linearInput) - linearDeadzone) / (1.0 - linearDeadzone)));
        }

        if (Math.Abs(angularInput) < angularDeadzone)
            angularInput = 0.0;
        else
        {
            // Normalize after deadzone
            var sign = Math.Sign(angularInput);
            angularInput = (sign * ((Math.Abs(angularInput) - angularDeadzone) / (1.0 - angularDeadzone)));
        }

        // Calculate final velocities
        var linearVelocity = _config.MinLinearVelocity + linearInput * linearVelocityRange * speedFactor;
        var angularVelocity = _config.MinAngularVelocity + angularInput * angularVelocityRange * speedFactor;

        twist.Linear = new Vector3(linearVelocity, 0, 0);
        twist.Angular = new Vector3(0, 0, angularVelocity);

        return twist;
    }

    /// <summary>
    /// Handle lift module control from keyboard
    /// Q: Lift up, Z: Lift down
    /// </summary>
    private void HandleLiftModuleControlFromKeyboard(CancellationToken cancellationToken)
    {
        // Check if lift module is ready
        if (_liftModule.State != LiftModuleState.Ready)
            return;

        bool liftUp, liftDown;
        lock (_lock)
        {
            liftUp = _keyStates.GetValueOrDefault(ConsoleKey.Q, false);
            liftDown = _keyStates.GetValueOrDefault(ConsoleKey.Z, false);
        }

        try
        {
            // Handle lift up
            if (liftUp)
            {
                var task = _liftModule.LiftUpAsync(cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling lift up (200ms) - CPU may be overloaded");
                }
            }
            // Handle lift down
            else if (liftDown)
            {
                var task = _liftModule.LiftDownAsync(cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling lift down (200ms) - CPU may be overloaded");
                }
            }
        }
        catch (OperationCanceledException)
        {
            throw;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error handling lift module control from keyboard");
        }
    }

    /// <summary>
    /// Handle rotation module control from keyboard
    /// E: Rotate left, C: Rotate right
    /// </summary>
    private void HandleRotationModuleControlFromKeyboard(CancellationToken cancellationToken)
    {
        // Check if rotation module is ready
        if (_rotationModule.State != RotationModuleState.Ready)
            return;

        bool rotateLeft, rotateRight;
        lock (_lock)
        {
            rotateLeft = _keyStates.GetValueOrDefault(ConsoleKey.E, false);
            rotateRight = _keyStates.GetValueOrDefault(ConsoleKey.C, false);
        }

        try
        {
            // Handle rotate left (90 degrees)
            if (rotateLeft)
            {
                var task = _rotationModule.RotateOffsetAsync(90.0, cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling rotate left (200ms) - CPU may be overloaded");
                }
            }
            // Handle rotate right (-90 degrees)
            else if (rotateRight)
            {
                var task = _rotationModule.RotateOffsetAsync(-90.0, cancellationToken);

                if (!task.Wait(TimeSpan.FromMilliseconds(200), CancellationToken.None))
                {
                    _logger.LogWarning("Timeout handling rotate right (200ms) - CPU may be overloaded");
                }
            }
        }
        catch (OperationCanceledException)
        {
            throw;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error handling rotation module control from keyboard");
        }
    }

    /// <summary>
    /// Start keyboard listener thread
    /// Đọc keyboard input và cập nhật _keyStates
    /// </summary>
    private void StartKeyboardListener(CancellationToken cancellationToken)
    {
        if (_keyboardThread != null && _keyboardThread.IsAlive)
        {
            _logger.LogWarning("Keyboard listener already running");
            return;
        }

        _keyboardReady = false;

        // Create keyboard listener thread based on OS
        if (OperatingSystem.IsWindows())
        {
            StartWindowsKeyboardListener(cancellationToken);
        }
        else if (OperatingSystem.IsLinux())
        {
            StartLinuxKeyboardListener(cancellationToken);
        }
        else
        {
            _logger.LogError("Keyboard input not supported on {OS}", Environment.OSVersion.Platform);
            throw new PlatformNotSupportedException($"Keyboard input not supported on {Environment.OSVersion.Platform}");
        }
    }

    /// <summary>
    /// Start Windows keyboard listener using Console.ReadKey
    /// TODO: Replace with low-level keyboard hook (SetWindowsHookEx) for better performance
    /// </summary>
    private void StartWindowsKeyboardListener(CancellationToken cancellationToken)
    {
        _keyboardThread = new Thread(() =>
        {
            try
            {
                _keyboardReady = true;
                _logger.LogInformation("Windows keyboard listener started");

                while (!cancellationToken.IsCancellationRequested)
                {
                    if (Console.KeyAvailable)
                    {
                        var keyInfo = Console.ReadKey(intercept: true);

                        lock (_lock)
                        {
                            // Set key state to true
                            _keyStates[keyInfo.Key] = true;

                            // Handle speed adjustment
                            if (keyInfo.Key == ConsoleKey.Add || keyInfo.KeyChar == '+')
                            {
                                _keyboardSpeed = Math.Min(100, _keyboardSpeed + 10);
                                _logger.LogDebug("Speed increased to {Speed}%", _keyboardSpeed);
                            }
                            else if (keyInfo.Key == ConsoleKey.Subtract || keyInfo.KeyChar == '-')
                            {
                                _keyboardSpeed = Math.Max(0, _keyboardSpeed - 10);
                                _logger.LogDebug("Speed decreased to {Speed}%", _keyboardSpeed);
                            }
                        }

                        // Clear key state after short delay (simulate key release)
                        Task.Delay(100, cancellationToken).ContinueWith(t =>
                        {
                            if (!t.IsCanceled)
                            {
                                lock (_lock)
                                {
                                    _keyStates[keyInfo.Key] = false;
                                }
                            }
                        }, cancellationToken);
                    }
                    else
                    {
                        Thread.Sleep(10);
                    }
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in Windows keyboard listener");
                _keyboardReady = false;
            }
        })
        {
            Name = "KeyboardListener-Windows",
            IsBackground = true
        };

        _keyboardThread.Start();
    }

    /// <summary>
    /// Find keyboard device among /dev/input/event* devices
    /// Returns file descriptor or -1 if not found
    /// </summary>
    private int FindKeyboardDevice()
    {
        for (int i = 0; i < INPUT_DEVICE_MAX; i++)
        {
            string devicePath = $"/dev/input/event{i}";

            int fd = EvdevNative.open(devicePath, EvdevNative.O_RDONLY | EvdevNative.O_NONBLOCK);
            if (fd < 0)
                continue;

            try
            {
                byte[] nameBuffer = new byte[256];
                int result = EvdevNative.ioctl(fd, EvdevNative.EVIOCGNAME_256, nameBuffer);

                if (result >= 0)
                {
                    string deviceName = Encoding.UTF8.GetString(nameBuffer, 0, result).TrimEnd('\0');
                    _logger.LogDebug("Found input device {Device}: {Name}", devicePath, deviceName);

                    if (deviceName.Contains("keyboard", StringComparison.OrdinalIgnoreCase) ||
                        deviceName.Contains("kbd", StringComparison.OrdinalIgnoreCase))
                    {
                        _logger.LogInformation("Selected keyboard device: {Device} ({Name})", devicePath, deviceName);
                        return fd;
                    }
                }

                EvdevNative.close(fd);
            }
            catch (Exception ex)
            {
                _logger.LogDebug(ex, "Error querying device {Device}", devicePath);
                EvdevNative.close(fd);
            }
        }

        return -1;
    }

    /// <summary>
    /// Process a key event from evdev
    /// </summary>
    private void ProcessKeyEvent(ushort keyCode, int value)
    {
        if (!LinuxKeyToConsoleKey.TryGetValue(keyCode, out ConsoleKey consoleKey))
            return;

        lock (_lock)
        {
            if (value == EvdevNative.KEY_PRESS || value == EvdevNative.KEY_REPEAT)
            {
                bool wasAlreadyPressed = _keyStates.GetValueOrDefault(consoleKey, false);
                _keyStates[consoleKey] = true;

                if (!wasAlreadyPressed)
                {
                    if (consoleKey == ConsoleKey.Add || keyCode == EvdevNative.KEY_EQUAL)
                    {
                        _keyboardSpeed = Math.Min(100, _keyboardSpeed + 10);
                        _logger.LogDebug("Speed increased to {Speed}%", _keyboardSpeed);
                    }
                    else if (consoleKey == ConsoleKey.Subtract || keyCode == EvdevNative.KEY_MINUS)
                    {
                        _keyboardSpeed = Math.Max(0, _keyboardSpeed - 10);
                        _logger.LogDebug("Speed decreased to {Speed}%", _keyboardSpeed);
                    }
                }
            }
            else if (value == EvdevNative.KEY_RELEASE)
            {
                _keyStates[consoleKey] = false;
            }
        }
    }

    /// <summary>
    /// Fallback keyboard listener using Console.ReadKey
    /// </summary>
    private void FallbackToConsoleReadKey(CancellationToken cancellationToken)
    {
        _keyboardReady = true;

        while (!cancellationToken.IsCancellationRequested)
        {
            if (Console.KeyAvailable)
            {
                var keyInfo = Console.ReadKey(intercept: true);

                lock (_lock)
                {
                    _keyStates[keyInfo.Key] = true;

                    if (keyInfo.Key == ConsoleKey.Add || keyInfo.KeyChar == '+')
                    {
                        _keyboardSpeed = Math.Min(100, _keyboardSpeed + 10);
                        _logger.LogDebug("Speed increased to {Speed}%", _keyboardSpeed);
                    }
                    else if (keyInfo.Key == ConsoleKey.Subtract || keyInfo.KeyChar == '-')
                    {
                        _keyboardSpeed = Math.Max(0, _keyboardSpeed - 10);
                        _logger.LogDebug("Speed decreased to {Speed}%", _keyboardSpeed);
                    }
                }

                Task.Delay(100, cancellationToken).ContinueWith(t =>
                {
                    if (!t.IsCanceled)
                    {
                        lock (_lock)
                        {
                            _keyStates[keyInfo.Key] = false;
                        }
                    }
                }, cancellationToken);
            }
            else
            {
                Thread.Sleep(10);
            }
        }
    }

    /// <summary>
    /// Start Linux keyboard listener reading from /dev/input/event*
    /// Falls back to Console.ReadKey if evdev unavailable
    /// </summary>
    private void StartLinuxKeyboardListener(CancellationToken cancellationToken)
    {
        _keyboardThread = new Thread(() =>
        {
            try
            {
                _evdevFd = FindKeyboardDevice();

                if (_evdevFd >= 0)
                {
                    _logger.LogInformation("Linux evdev keyboard listener started (fd={Fd})", _evdevFd);
                    _keyboardReady = true;

                    var inputEvent = new EvdevNative.input_event();
                    int eventSize = Marshal.SizeOf<EvdevNative.input_event>();

                    while (!cancellationToken.IsCancellationRequested)
                    {
                        int bytesRead = EvdevNative.read(_evdevFd, ref inputEvent, eventSize);

                        if (bytesRead < 0)
                        {
                            int errno = EvdevNative.GetLastError();

                            if (errno == EvdevNative.EAGAIN)
                            {
                                Thread.Sleep(10);
                                continue;
                            }

                            _logger.LogError("Error reading evdev (errno={Errno})", errno);
                            break;
                        }

                        if (bytesRead != eventSize)
                        {
                            _logger.LogWarning("Partial read: {Bytes}/{Expected} bytes", bytesRead, eventSize);
                            continue;
                        }

                        if (inputEvent.type == EvdevNative.EV_KEY)
                        {
                            ProcessKeyEvent(inputEvent.code, inputEvent.value);
                        }
                    }
                }
                else
                {
                    _logger.LogWarning(
                        "Linux evdev not available (no keyboard found or permission denied). " +
                        "Using Console.ReadKey fallback. " +
                        "To use evdev: sudo usermod -aG input $USER && reboot");

                    FallbackToConsoleReadKey(cancellationToken);
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in Linux keyboard listener");
                _keyboardReady = false;
            }
            finally
            {
                if (_evdevFd >= 0)
                {
                    EvdevNative.close(_evdevFd);
                    _evdevFd = -1;
                }
            }
        })
        {
            Name = "KeyboardListener-Linux-Evdev",
            IsBackground = true
        };

        _keyboardThread.Start();
    }

    /// <summary>
    /// Stop keyboard listener thread
    /// </summary>
    private void StopKeyboardListener()
    {
        _keyboardReady = false;

        if (_keyboardThread != null && _keyboardThread.IsAlive)
        {
            try
            {
                if (!_keyboardThread.Join(TimeSpan.FromSeconds(2)))
                {
                    _logger.LogWarning("Timeout waiting for keyboard listener thread to stop");
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error stopping keyboard listener thread");
            }

            _keyboardThread = null;
        }

        lock (_lock)
        {
            _keyStates.Clear();
        }
    }

    private void StopRobot()
    {
        var zeroTwist = new Twist();
        lock (_lock)
        {
            _currentTwist = zeroTwist;
        }

        // Send zero velocity to IInverseKinematics if available
        try
        {
            var task = _inverseKinematics.SetVelocityAsync(zeroTwist);
            if (!task.Wait(TimeSpan.FromMilliseconds(50), CancellationToken.None))
            {
                _logger.LogWarning("Timeout stopping robot (50ms) - CPU may be overloaded");
            }
        }
        catch (AggregateException ex)
        {
            _logger.LogError(ex.InnerException ?? ex, "Error stopping robot");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error stopping robot");
        }
    }

    #region IHostedService

    /// <summary>
    /// IHostedService.StartAsync - Chỉ khởi tạo phần cứng (device, keyboard)
    /// KHÔNG start update loop - module manager sẽ gọi Start() khi sẵn sàng
    /// </summary>
    public async Task StartAsync(CancellationToken cancellationToken)
    {
        try
        {
            if (!_config.Enable) return;

            if (_config.UsingKeyboard)
            {
                // Start keyboard listener thread
                StartKeyboardListener(cancellationToken);
                _logger.LogInformation("Keyboard input initialized successfully. Keys: W/S (linear), A/D (angular), Q/Z (lift), E/C (rotate), +/- (speed), Space (stop)");

                // Wait for modules to be ready
                if(_liftModule.Enable)
                {
                    while (!_liftModule.IsReady && !cancellationToken.IsCancellationRequested)
                    {
                        await Task.Delay(500, CancellationToken.None);
                    }
                }

                if(_rotationModule.Enable)
                {
                    while (!_rotationModule.IsReady && !cancellationToken.IsCancellationRequested)
                    {
                        await Task.Delay(500, CancellationToken.None);
                    }
                }
                _currentState = ManualControlState.Active;
                Start();
                /*if (_liftModule.IsReady && _rotationModule.IsReady && !cancellationToken.IsCancellationRequested)
                {
                    Start();
                }*/
            }
            else
            {
                // Đợi DeviceProvider kết nối xong tất cả devices
                var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);

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

                // Get RF Handle device
                var device = _deviceProvider.GetDevice(_config.RfHandleDeviceId);
                if (device is not IRfHandle rfHandle)
                {
                    _logger.LogError("RF Handle device '{DeviceId}' is not an IRfHandle", _config.RfHandleDeviceId);
                    return;
                }

                _rfHandle = rfHandle;
                _rfHandle.Updated += OnRfHandleUpdated;
            }
            _isRunning = true;
            _logger.LogInformation("ManualControlService hardware initialized. Waiting for Start() to begin update loop.");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error initializing ManualControlService hardware");
        }
    }

    /// <summary>
    /// IHostedService.StopAsync - Cleanup khi ứng dụng tắt
    /// </summary>
    public Task StopAsync(CancellationToken cancellationToken)
    {
        try
        {
            if (!_config.Enable) return Task.CompletedTask;

            Stop();

            if (_config.UsingKeyboard)
            {
                StopKeyboardListener();
            }
            else if (_rfHandle != null)
            {
                _rfHandle.Updated -= OnRfHandleUpdated;
            }

            // Stop update thread and wait for it to finish
            StopUpdateLoop();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "ManualControlService: Error during StopAsync()");
            throw;
        }

        return Task.CompletedTask;
    }

    private void OnRfHandleUpdated()
    {
        // State is now determined in UpdateLoop based on RF Handle data
        // This event can be used for external notifications if needed
    }

    #endregion

    #region Public Control Methods

    /// <summary>
    /// Bắt đầu update loop - được gọi bởi module manager khi hệ thống sẵn sàng
    /// </summary>
    public void Start()
    {
        if (!_config.Enable) return;

        if (_updateThread != null && _updateThread.IsAlive)
        {
            _logger.LogDebug("ManualControlService: Update loop already running");
            return;
        }

        StartUpdateLoop();
        _logger.LogInformation("ManualControlService: Update loop started");
    }

    /// <summary>
    /// Dừng update loop và stop robot - được gọi bởi module manager
    /// </summary>
    public void Stop()
    {
        if (!_config.Enable) return;

        _logger.LogInformation("ManualControlService: Stopping update loop");
        StopUpdateLoop();
        StopRobot();
    }

    /// <summary>
    /// Kiểm tra update loop có đang chạy không (thread-safe)
    /// </summary>
    public bool IsRunning => _isRunning;

    /// <summary>
    /// Sets the ManualControl state externally (called by RobotStateMachine)
    /// Only Maintenance and Override states can be set externally
    /// To exit these states, call with null or let RF Handle conditions reset it
    /// </summary>
    public void SetState(ManualControlState? state)
    {
        if (state.HasValue && state.Value != ManualControlState.Maintenance && state.Value != ManualControlState.Override)
        {
            _logger.LogWarning("SetState called with invalid state {State}. Only Maintenance and Override can be set externally.", state);
            return;
        }

        lock (_lock)
        {
            var previousExternalState = _externallySetState;
            _externallySetState = state;

            if (state.HasValue)
            {
                _logger.LogInformation("ManualControlService state externally set to {State}", state.Value);
            }
            else if (previousExternalState.HasValue)
            {
                _logger.LogInformation("ManualControlService external state cleared (was {PreviousState})", previousExternalState.Value);
            }
        }
    }

    /// <summary>
    /// Clears externally set state, allowing state to be determined by RF Handle data
    /// Called by RobotStateMachine when exiting Service or Remote_Override states
    /// </summary>
    public void ClearExternalState()
    {
        SetState(null);
        // Reset _previousRfMode so next UpdateFromDeviceLoop cycle detects current RF mode as "changed"
        // This ensures re-entry to Service/Remote_Override after Stop release or mode exit
        lock (_lock)
        {
            _previousRfMode = RFMode.None;
        }
    }

    #endregion

    #region IDisposable

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

        _disposed = true;

        try
        {
            if (_config.Enable)
            {
                StopUpdateLoop();
                StopRobot();

                // Stop keyboard listener if using keyboard
                if (_config.UsingKeyboard)
                {
                    StopKeyboardListener();
                }

                // Unsubscribe from RF Handle events
                if (_rfHandle != null)
                {
                    _rfHandle.Updated -= OnRfHandleUpdated;
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing ManualControlService");
        }
        GC.SuppressFinalize(this);
    }

    #endregion
}

/// <summary>
/// Status của RF Handle (for SignalR)
/// </summary>
public class RfHandleStatus
{
    public int Heartbeat { get; set; }
    public bool Ready { get; set; }
    public bool Locked { get; set; }
    public bool EStop { get; set; }
    public bool Enable { get; set; }
    public int Speed { get; set; }
    public double Linear { get; set; }
    public double Angular { get; set; }
    public string Mode { get; set; } = string.Empty;
    public DateTime LastUpdateTime { get; set; }
}