using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Xloc;
using System;
using System.Runtime.InteropServices;
using System.Threading;
using System.Threading.Tasks;
using RobotNet10.Shared.Sensor;
using RobotNet10.Shared.Geometry;


namespace RobotNet10.RobotApp.Navigation;

/// <summary>
/// Integration service to run the navigation program using the Navigation C API
/// Integrates with XlocIntegrationService for pose updates and provides navigation control
/// </summary>
public class NavigationIntegrationService : IHostedService, IDisposable
{
    private readonly NavigationIntegrationConfiguration _config;
    private readonly IDeviceProvider _deviceProvider;
    private readonly ILogger<NavigationIntegrationService> _logger;
    private readonly XlocIntegrationService? _xlocService;
    private readonly OdometryService? _odometryService;
    private readonly PS5ControllerService? _ps5ControllerService;
    private readonly IInverseKinematics? _inverseKinematics;
    private readonly object _lock = new();
    private readonly SemaphoreSlim _twistDispatchSemaphore = new SemaphoreSlim(1, 1); // Ensure only one thread sends twist at a time
    
    private NavigationClient? _navigationClient;
    private Timer? _updateTimer;  // Timer for periodic updates (pose, feedback, etc.)
    
    // Cached device references
    private ILidar? _lidarDevice1;
    private ILidar? _lidarDevice2;  // Second lidar (right side)
    private ILidar? _lidarDevice3;  // Third lidar (left side)
    
    private bool _disposed = false;
    private bool _isInitialized = false;
    private string? _lastActiveMapName = null; // Track last active map to detect changes
    private volatile bool _hasActiveGoal = false;
    /// <summary>Raised when navigation reaches a terminal state (Succeeded, Preempted, Recalled, Aborted, Rejected, Lost).</summary>
    public event Action<NavigationState>? OnNavigationResult;

    public NavigationIntegrationService(
        IConfiguration configuration,
        IDeviceProvider deviceProvider,
        ILogger<NavigationIntegrationService> logger,
        XlocIntegrationService? xlocService = null,
        OdometryService? odometryService = null,
        NavigationClient? navigationClient = null,
        PS5ControllerService? ps5ControllerService = null,
        IInverseKinematics? inverseKinematics = null)
    {
        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
        _xlocService = xlocService;
        _odometryService = odometryService;
        _navigationClient = navigationClient;
        _ps5ControllerService = ps5ControllerService;
        _inverseKinematics = inverseKinematics;

        // Load configuration
        var configSection = configuration.GetSection("Navigation:Integration");
        if (!configSection.Exists())
        {
            _logger.LogWarning("Configuration section 'Navigation:Integration' not found. Using defaults.");
            _config = new NavigationIntegrationConfiguration();
        }
        else
        {
            _config = new NavigationIntegrationConfiguration();
            configSection.Bind(_config);
            
            // Log footprint configuration for debugging
            if (_config.RobotFootprint != null && _config.RobotFootprint.Length > 0)
            {
                _logger.LogInformation("Loaded RobotFootprint from config: {Count} points", _config.RobotFootprint.Length);
                for (int i = 0; i < _config.RobotFootprint.Length; i++)
                {
                    var point = _config.RobotFootprint[i];
                    _logger.LogInformation("  Point[{Index}]: X={X}, Y={Y}, Z={Z}", i, point.X, point.Y, point.Z);
                }
            }
            else
            {
                _logger.LogWarning("RobotFootprint not loaded from config or is empty");
            }
        }

        ValidateConfiguration();
    }

    private void ValidateConfiguration()
    {
        if (_config.Enabled)
        {
            if (_config.UpdateIntervalMs <= 0 || _config.UpdateIntervalMs > 1000)
            {
                _logger.LogWarning("UpdateIntervalMs {Interval} is out of range [1-1000]. Using default 100 ms.", _config.UpdateIntervalMs);
                _config.UpdateIntervalMs = 100;
            }

            if (_config.EnableNavigationIntegration && _navigationClient == null)
            {
                _logger.LogWarning("Navigation integration is enabled but NavigationIntegrationService is not available. Will be disabled.");
                _config.EnableNavigationIntegration = false;
            }
        }
    }

    public async Task StartAsync(CancellationToken cancellationToken)
    {
        if (!_config.Enabled)
        {
            _logger.LogInformation("NavigationIntegrationService is disabled in configuration");
            return;
        }

        _logger.LogInformation("Starting NavigationIntegrationService (async, non-blocking)...");

        try
        {
            // IMPORTANT: Run initialization in a BACKGROUND TASK to avoid blocking the main application startup
            // This allows the main app to continue while navigation initializes asynchronously
            // Navigation will retry automatically if devices aren't ready yet
            _ = Task.Run(async () => await InitializeInBackgroundAsync(cancellationToken), cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error starting NavigationIntegrationService background task: {Message}", ex.Message);
            // Don't throw - allow service to retry later
        }

        // Return immediately without waiting for initialization
        await Task.CompletedTask;
    }

    /// <summary>
    /// Initialize NavigationIntegrationService in background without blocking
    /// - Waits for devices to connect
    /// - Retries automatically if devices not ready
    /// - Navigation will NOT block the main app startup
    /// </summary>
    private async Task InitializeInBackgroundAsync(CancellationToken cancellationToken)
    {
        try
        {
            // Wait for devices to be connected (with timeout for faster feedback)
            _logger.LogInformation("[NAV] Waiting for devices to be connected (background task)...");
            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(30), cancellationToken);

            if (!connected)
            {
                _logger.LogWarning("[NAV] Timeout waiting for devices. NavigationIntegrationService will retry initialization in background.");
                _ = Task.Run(async () => await RetryInitializationAsync(cancellationToken), cancellationToken);
                return;
            }

            _logger.LogInformation("[NAV] Devices connected. Initializing NavigationIntegrationService...");
            await InitializeAsync(cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "[NAV] Error in background initialization: {Message}", ex.Message);
            // Retry automatically
            _ = Task.Run(async () => await RetryInitializationAsync(cancellationToken), cancellationToken);
        }
    }

    private async Task RetryInitializationAsync(CancellationToken cancellationToken)
    {
        const int maxRetries = 60; // 5 minutes with 5 second intervals
        int retryCount = 0;

        while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested && !_isInitialized)
        {
            await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);

            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(1), cancellationToken);
            if (connected)
            {
                _logger.LogInformation("[NAV] Devices are now connected. Initializing NavigationIntegrationService...");
                await InitializeAsync(cancellationToken);
                return;
            }

            retryCount++;
            if (retryCount % 12 == 0) // Log every minute
            {
                _logger.LogInformation("[NAV] Still waiting for devices... (attempt {Attempt}/{MaxAttempts})",
                    retryCount, maxRetries);
            }
        }

        if (retryCount >= maxRetries)
        {
            _logger.LogWarning("[NAV] Timeout waiting for devices. NavigationIntegrationService will not be initialized.");
        }
    }

    private async Task InitializeAsync(CancellationToken cancellationToken)
    {
        bool shouldInitialize = false;
        NavigationClient? navigationClient = null;

        lock (_lock)
        {
            if (_isInitialized)
                return;

            shouldInitialize = true;
            // Verify xloc service and client are available before creating navigation client
            if (_xlocService?.XlocClient == null)
            {
                throw new InvalidOperationException("XlocService or XlocClient is not available. XlocClient must be initialized before NavigationClient.");
            }

            // Get TF buffer from XlocClient (which received it from TF3BufferManager)
            IntPtr tfBuffer = _xlocService.XlocClient.TfBuffer;
            if (tfBuffer == IntPtr.Zero)
            {
                throw new InvalidOperationException("TF buffer is not initialized. Ensure TF3BufferManager is initialized first.");
            }

            // Create navigation client with TF buffer
            navigationClient = new NavigationClient(tfBuffer, _logger);
            _navigationClient = navigationClient;
        }

        if (!shouldInitialize)
            return;

        try
        {
            // Initialize navigation client (outside lock to avoid deadlock)
            navigationClient.Initialize();
            _logger.LogInformation("NavigationClient initialized successfully");

            // Set robot footprint if configured
            if (_config.RobotFootprint != null && _config.RobotFootprint.Length > 0)
            {
                var footprint = new Point[_config.RobotFootprint.Length];
                _logger.LogInformation("Setting robot footprint with {Count} points. Config points:", footprint.Length);
                for (int i = 0; i < _config.RobotFootprint.Length; i++)
                {
                    var point = _config.RobotFootprint[i];
                    _logger.LogInformation("  Config Point[{Index}]: X={X}, Y={Y}, Z={Z}", i, point.X, point.Y, point.Z);
                    footprint[i] = new Point
                    {
                        x = point.X,
                        y = point.Y,
                        z = point.Z
                    };
                    _logger.LogInformation("  Footprint Point[{Index}]: x={X}, y={Y}, z={Z}", i, footprint[i].x, footprint[i].y, footprint[i].z);
                }

                if (navigationClient.SetRobotFootprint(footprint))
                {
                    _logger.LogInformation("Robot footprint set successfully ({Count} points)", footprint.Length);
                }
                else
                {
                    _logger.LogWarning("Failed to set robot footprint");
                }
            }
            else
            {
                _logger.LogWarning("Robot footprint not configured or empty. Navigation will use default footprint");
            }

            // Wait for navigation to be ready before adding data (outside lock to allow await)
            _logger.LogInformation("Waiting for navigation to be ready...");
            int readyCheckCount = 0;
            const int maxReadyChecks = 50; // Wait up to 5 seconds (50 * 100ms)
            bool navigationReady = false;
            
            while (readyCheckCount < maxReadyChecks && !cancellationToken.IsCancellationRequested)
            {
                var feedback = navigationClient.GetFeedback();
                if (feedback != null && feedback.IsReady)
                {
                    navigationReady = true;
                    _logger.LogInformation("Navigation is ready (is_ready = true)");
                    break;
                }
                await Task.Delay(100, cancellationToken);
                readyCheckCount++;
            }

            if (!navigationReady)
            {
                _logger.LogWarning("Navigation not ready after {Count} checks. Will continue but data dispatch may fail.", maxReadyChecks);
            }

            // Add static map from Xloc if available (only if navigation is ready)
            // if (_config.EnableStaticMap && _xlocService != null)
            // {
            //     try
            //     {
            //         var mapData = _xlocService.GetStaticGridMap();
            //         if (mapData != null)
            //         {
            //             if (navigationClient.DispatchStaticMap(mapData, "map"))
            //             {
            //                 _logger.LogInformation("Static map added successfully from Xloc");
            //             }
            //             else
            //             {
            //                 _logger.LogWarning("Failed to add static map to navigation");
            //             }
            //         }
            //         else
            //         {
            //             _logger.LogWarning("Static map requested but not available from Xloc");
            //         }
            //     }
            //     catch (Exception ex)
            //     {
            //         _logger.LogError(ex, "Error adding static map from Xloc");
            //     }
            // }
            // else if (_config.EnableStaticMap && !navigationReady)
            // {
            //     _logger.LogWarning("Static map requested but navigation is not ready yet");
            // }

            // Get device references for dispatch loops
            ILidar? lidarDevice1 = null;
            if (_config.EnableLaserScan1 && !string.IsNullOrEmpty(_config.Lidar1DeviceId))
            {
                var device = _deviceProvider.GetDevice(_config.Lidar1DeviceId);
                if (device is ILidar lidar && device.IsConnected)
                {
                    lidarDevice1 = lidar;
                    _logger.LogInformation("LIDAR device '{DeviceId}' found and connected", _config.Lidar1DeviceId);
                }
                else
                {
                    _logger.LogWarning("LIDAR device '{DeviceId}' not found or not connected. Laser scan dispatch will be disabled.",
                        _config.Lidar1DeviceId);
                }
            }

            // Second lidar (right side)
            ILidar? lidarDevice2 = null;
            if (_config.EnableLaserScan2 && !string.IsNullOrEmpty(_config.Lidar2DeviceId))
            {
                var device2 = _deviceProvider.GetDevice(_config.Lidar2DeviceId);
                if (device2 is ILidar lidar2 && device2.IsConnected)
                {
                    lidarDevice2 = lidar2;
                    _logger.LogInformation("LIDAR 2 device '{DeviceId}' found and connected", _config.Lidar2DeviceId);
                }
                else
                {
                    _logger.LogWarning("LIDAR 2 device '{DeviceId}' not found or not connected. Laser scan 2 dispatch will be disabled.",
                        _config.Lidar2DeviceId);
                }
            }

            // Third lidar (left side)
            ILidar? lidarDevice3 = null;
            if (_config.EnableLaserScan3 && !string.IsNullOrEmpty(_config.Lidar3DeviceId))
            {
                var device3 = _deviceProvider.GetDevice(_config.Lidar3DeviceId);
                if (device3 is ILidar lidar3 && device3.IsConnected)
                {
                    lidarDevice3 = lidar3;
                    _logger.LogInformation("LIDAR 3 device '{DeviceId}' found and connected", _config.Lidar3DeviceId);
                }
                else
                {
                    _logger.LogWarning("LIDAR 3 device '{DeviceId}' not found or not connected. Laser scan 3 dispatch will be disabled.",
                        _config.Lidar3DeviceId);
                }
            }

            // Start dispatch loops as background tasks (similar to Xloc)
            if (_config.EnableOdometryDispatch && _odometryService != null)
            {
                _ = Task.Run(async () => await OdometryDispatchLoopAsync(cancellationToken), cancellationToken);
                _logger.LogInformation("Odometry dispatch loop started at {Rate} Hz", 1000.0 / _config.OdomDispatchIntervalMs);
            }

            if (_config.EnableLaserScan1 && lidarDevice1 != null)
            {   
                _ = Task.Run(async () => await LaserScan1DispatchLoopAsync(cancellationToken), cancellationToken);
                _logger.LogInformation("Laser scan 1 dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
            }

            if (_config.EnableLaserScan2 && lidarDevice2 != null)
            {
                _ = Task.Run(async () => await LaserScan2DispatchLoopAsync(cancellationToken), cancellationToken);
                _logger.LogInformation("Laser scan 2 dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
            }

            if (_config.EnableLaserScan3 && lidarDevice3 != null)
            {
                _ = Task.Run(async () => await LaserScan3DispatchLoopAsync(cancellationToken), cancellationToken);
                _logger.LogInformation("Laser scan 3 dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
            }

            // Dispatch grid map once during initialization if enabled
            if (_config.EnableGridMapDispatch && _xlocService != null)
            {
                _ = Task.Run(async () => await DispatchGridMapOnceAsync(cancellationToken), cancellationToken);
                _logger.LogInformation("GridMap dispatch scheduled for initialization");
            }

            // if (_config.EnableTwistLinear && _odometryService != null)
            // {
            //     _ = Task.Run(async () => await TwistLinearDispatchLoopAsync(cancellationToken), cancellationToken);
            //     _logger.LogInformation("Twist linear dispatch loop started at {Rate} Hz", 1000.0 / _config.TwistDispatchIntervalMs);
            // }

            if (_config.EnableNavigationTwistDispatch && _inverseKinematics != null)
            {
                _ = Task.Run(async () => await NavigationTwistDispatchLoopAsync(cancellationToken), cancellationToken);
                _logger.LogInformation("Navigation twist dispatch loop started at {Rate} Hz", 1000.0 / _config.NavigationTwistDispatchIntervalMs);
            }

            // Set initialized flag and assign device references FIRST (inside lock for thread safety)
            // This ensures _isInitialized is true before timer starts
            lock (_lock)
            {
                _lidarDevice1 = lidarDevice1;
                _lidarDevice2 = lidarDevice2;
                _lidarDevice3 = lidarDevice3;
                _isInitialized = true;
            }

            // Start update timer for periodic pose/feedback updates
            // Create timer AFTER _isInitialized is set to prevent race condition
            Timer? updateTimer = new Timer(UpdateNavigationState, null,
                TimeSpan.FromMilliseconds(_config.UpdateIntervalMs),
                TimeSpan.FromMilliseconds(_config.UpdateIntervalMs));
            _logger.LogInformation("Navigation update timer started (interval: {Interval} ms)", _config.UpdateIntervalMs);

            // Assign timer reference (inside lock for thread safety)
            lock (_lock)
            {
                _updateTimer = updateTimer;
            }

            _logger.LogInformation("✅ NavigationIntegrationService initialized successfully");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Failed to initialize NavigationIntegrationService");
            lock (_lock)
            {
                _isInitialized = false;
                _navigationClient = null;
            }
            throw;
        }
    }

    /// <summary>
    /// Periodic update callback - updates navigation state, pose, and feedback
    /// </summary>
    private void UpdateNavigationState(object? state)
    {
        NavigationClient? client = null;

        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return;
            
            client = _navigationClient;
        }

        if (client == null)
            return;

        // Check if NavigationClient is fully initialized before using it
        // This prevents calling methods when _handle is still IntPtr.Zero
        if (!client.IsInitialized)
        {
            _logger.LogTrace("NavigationClient not fully initialized yet, skipping update");
            return;
        }

        try
        {
            // Update pose from Xloc if enabled
            if (_config.EnableNavigationIntegration && client != null)
            {
                var pose = client.GetRobotPoseStamped();
                if (pose.HasValue)
                {
                    var (x, y, z, qx, qy, qz, qw, frameId) = pose.Value;
                    
                    // Validate quaternion: check if it's normalized (norm should be > 0)
                    // If quaternion is invalid (all zeros), skip update to keep existing transform
                    double quatNorm = Math.Sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
                    
                    if (quatNorm < 1e-6) // Quaternion is invalid (all zeros or near-zero)
                    {
                        // Skip update if quaternion is invalid - keep the existing transform
                        // This prevents overwriting a valid transform with an invalid one
                        _logger.LogTrace("Invalid quaternion from Xloc (norm={Norm:F6}), skipping transform update", quatNorm);
                        // Don't update transform, keep the existing one - continue to get feedback
                    }
                    else
                    {
                        double finalQx = qx;
                        double finalQy = qy;
                        double finalQz = qz;
                        double finalQw = qw;
                        
                        if (Math.Abs(quatNorm - 1.0) > 1e-3) // Quaternion is not normalized
                        {
                            // Normalize the quaternion
                            finalQx = qx / quatNorm;
                            finalQy = qy / quatNorm;
                            finalQz = qz / quatNorm;
                            finalQw = qw / quatNorm;
                            _logger.LogTrace("Quaternion normalized from norm {Norm:F6} to 1.0", quatNorm);
                        }
                        
                        // Update map -> odom transform from Xloc pose
                        // This allows navigation to get the robot's pose in map frame
                        client.UpdateMapToOdomTransform(x, y, z, finalQx, finalQy, finalQz, finalQw);
                        
                        _logger.LogTrace("Updated transform from Xloc pose: ({X:F2}, {Y:F2}, {Z:F2})", x, y, z);
                    }
                }
            }

            // Get and log navigation feedback periodically
            var feedback = client.GetFeedback();
            if (feedback != null)
            {
                _logger.LogTrace("Navigation state: {State}, Ready: {Ready}, GoalChecked: {GoalChecked}",
                    feedback.StateString, feedback.IsReady, feedback.GoalChecked);
                // Notify when goal completes so order controller can clear state and accept new orders
                if (_hasActiveGoal && IsTerminalState(feedback.NavigationState))
                {
                    _hasActiveGoal = false;
                    try { OnNavigationResult?.Invoke(feedback.NavigationState); }
                    catch (Exception ex) { _logger.LogError(ex, "OnNavigationResult subscriber error"); }
                }
            }

            // Check for map changes and dispatch if needed
            if (_config.EnableGridMapDispatch && _xlocService != null)
            {
                CheckAndDispatchGridMapIfChanged();
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error updating navigation state");
        }
    }

    private static bool IsTerminalState(NavigationState state)
    {
        return state is NavigationState.Succeeded or NavigationState.Preempted or NavigationState.Recalled
            or NavigationState.Aborted or NavigationState.Rejected or NavigationState.Lost;
    }

    public async Task StopAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Stopping NavigationIntegrationService...");

        try
        {
            // IMPORTANT: Set _isInitialized to false FIRST to stop update timer
            lock (_lock)
            {
                _isInitialized = false;
                
                // Dispose update timer
                _updateTimer?.Dispose();
                _updateTimer = null;
            }

            // Wait a bit for update loop to notice _isInitialized = false and stop
            _logger.LogInformation("Waiting for update loop to stop...");
            await Task.Delay(500, cancellationToken);

            // Cancel any active navigation goals
            if (_navigationClient != null)
            {
                _navigationClient.Cancel();
                _navigationClient.Dispose();
                _navigationClient = null;
                _logger.LogInformation("NavigationClient disposed successfully");
            }

            _logger.LogInformation("NavigationIntegrationService stopped successfully");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error stopping NavigationIntegrationService");
        }

        await Task.CompletedTask;
    }

    #region Public API Methods

    /// <summary>
    /// Get the navigation client (for external use)
    /// </summary>
    public NavigationClient? GetNavigationClient()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;
            
            return _navigationClient;
        }
    }

    /// <summary>
    /// Check if navigation is initialized
    /// </summary>
    public bool IsInitialized
    {
        get
        {
            lock (_lock)
            {
                return _isInitialized && _navigationClient != null && _navigationClient.IsInitialized;
            }
        }
    }

    /// <summary>
    /// Send a goal for the robot to navigate to
    /// </summary>
    public bool MoveTo(double x, double y, double z, double qx, double qy, double qz, double qw,
                       string frameId = "map",
                       double xyTolerance = 0.1,
                       double yawTolerance = 0.1)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
            {
                _logger.LogWarning("Cannot move to goal: NavigationIntegrationService not initialized");
                return false;
            }

            return _navigationClient.MoveTo(x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
        }
    }

    /// <summary>
    /// Send a goal for the robot to navigate to with order
    /// </summary>
    public bool MoveToOrder(IntPtr orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
                            string frameId = "map",
                            double xyTolerance = 0.1,
                            double yawTolerance = 0.1)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
            {
                _logger.LogWarning("Cannot move to goal with order: NavigationIntegrationService not initialized");
                return false;
            }

            try
            {
                _logger.LogInformation("NavigationIntegrationService.MoveToOrder called with order_handle: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}, tolerances: xy={XyTol}, yaw={YawTol}", 
                    orderHandle.ToInt64(), x, y, z, frameId, xyTolerance, yawTolerance);
                bool result = _navigationClient.MoveToOrder(orderHandle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
                _logger.LogInformation("NavigationIntegrationService.MoveToOrder completed with result: {Result}", result);
                if (result) _hasActiveGoal = true;
                return result;
            }
            catch (ArgumentException ex)
            {
                // Log at debug level since this is expected validation error, API endpoint will return error response
                _logger.LogDebug(ex, "Invalid order handle: {OrderHandle}", orderHandle.ToInt64());
                throw; // Re-throw to be caught by API endpoint
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Unexpected error in MoveToOrder: {Message}", ex.Message);
                throw; // Re-throw to be caught by API endpoint
            }
        }
    }

    /// <summary>
    /// Send a goal for the robot to navigate to with order (using OrderHandle wrapper)
    /// </summary>
    public bool MoveToOrder(OrderHandle orderHandle, double x, double y, double z, double qx, double qy, double qz, double qw,
                            string frameId = "map",
                            double xyTolerance = 0.1,
                            double yawTolerance = 0.1)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
            {
                _logger.LogWarning("Cannot move to goal with order: NavigationIntegrationService not initialized");
                return false;
            }

            try
            {
                _logger.LogInformation("NavigationIntegrationService.MoveToOrder called with OrderHandle wrapper: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}, tolerances: xy={XyTol}, yaw={YawTol}", 
                    orderHandle?.Handle.ToInt64() ?? 0, x, y, z, frameId, xyTolerance, yawTolerance);
                bool result = _navigationClient.MoveToOrder(orderHandle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
                _logger.LogInformation("NavigationIntegrationService.MoveToOrder completed with result: {Result}", result);
                if (result) _hasActiveGoal = true;
                return result;
            }
            catch (ArgumentException ex)
            {
                // Log at debug level since this is expected validation error, API endpoint will return error response
                _logger.LogDebug(ex, "Invalid order handle: {OrderHandle}", orderHandle?.Handle.ToInt64() ?? 0);
                throw; // Re-throw to be caught by API endpoint
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Unexpected error in MoveToOrder: {Message}", ex.Message);
                throw; // Re-throw to be caught by API endpoint
            }
        }
    }

    /// <summary>
    /// Send a goal for the robot to navigate to with order (using OrderData from JSON)
    /// </summary>
    public bool MoveToOrder(OrderData orderData, double x, double y, double z, double qx, double qy, double qz, double qw,
                            string frameId = "map",
                            double xyTolerance = 0.1,
                            double yawTolerance = 0.1)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null || !_navigationClient.IsInitialized)
            {
                _logger.LogWarning("Cannot move to goal with order: NavigationIntegrationService not initialized");
                return false;
            }

            try
            {
                _logger.LogInformation("NavigationIntegrationService.MoveToOrder called with OrderData: {OrderId}, goal: ({X}, {Y}, {Z}), frame: {FrameId}, tolerances: xy={XyTol}, yaw={YawTol}", 
                    orderData?.OrderId ?? "null", x, y, z, frameId, xyTolerance, yawTolerance);
                bool result = _navigationClient.MoveToOrder(orderData, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
                _logger.LogInformation("NavigationIntegrationService.MoveToOrder completed with result: {Result}", result);
                if (result) _hasActiveGoal = true;
                return result;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in MoveToOrder with OrderData: {Message}", ex.Message);
                throw;
            }
        }
    }

    /// <summary>
    /// Send a docking goal to a predefined marker
    /// </summary>
    public bool DockTo(string marker, double x, double y, double z, double qx, double qy, double qz, double qw,
                      string frameId = "map",
                      double xyTolerance = 0.05,
                      double yawTolerance = 0.05)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot dock to marker: NavigationIntegrationService not initialized");
                return false;
            }

            return _navigationClient.DockTo(marker, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance);
        }
    }

    /// <summary>
    /// Move straight toward the target position
    /// </summary>
    public bool MoveStraightTo(double distance)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot move straight: NavigationIntegrationService not initialized");
                return false;
            }

            return _navigationClient.MoveStraightTo(distance);
        }
    }

    /// <summary>
    /// Rotate in place to align with target orientation
    /// </summary>
    public bool RotateTo(double x, double y, double z, double qx, double qy, double qz, double qw,
                        string frameId = "map",
                        double yawTolerance = 0.1)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot rotate: NavigationIntegrationService not initialized");
                return false;
            }

            return _navigationClient.RotateTo(x, y, z, qx, qy, qz, qw, frameId, yawTolerance);
        }
    }

    /// <summary>
    /// Pause the robot's movement
    /// </summary>
    public void Pause()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot pause: NavigationIntegrationService not initialized");
                return;
            }

            _navigationClient.Pause();
        }
    }

    /// <summary>
    /// Resume motion after a pause
    /// </summary>
    public void Resume()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot resume: NavigationIntegrationService not initialized");
                return;
            }

            _navigationClient.Resume();
        }
    }

    /// <summary>
    /// Cancel the current goal and stop the robot
    /// </summary>
    public void Cancel()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot cancel: NavigationIntegrationService not initialized");
                return;
            }

            _navigationClient.Cancel();
        }
    }

    /// <summary>
    /// Send limited linear velocity command
    /// </summary>
    public bool SetTwistLinear(double linearX, double linearY, double linearZ)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot set linear twist: NavigationIntegrationService not initialized");
                return false;
            }

            return _navigationClient.SetTwistLinear(linearX, linearY, linearZ);
        }
    }

    /// <summary>
    /// Send limited angular velocity command
    /// </summary>
    public bool SetTwistAngular(double angularX, double angularY, double angularZ)
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
            {
                _logger.LogWarning("Cannot set angular twist: NavigationIntegrationService not initialized");
                return false;
            }

            return _navigationClient.SetTwistAngular(angularX, angularY, angularZ);
        }
    }

    /// <summary>
    /// Get the robot's current pose
    /// </summary>
    public (double x, double y, double z, double qx, double qy, double qz, double qw, string frameId)? GetRobotPose()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;

            return _navigationClient.GetRobotPoseStamped();
        }
    }

    /// <summary>
    /// Get the robot's current pose as 2D (x, y, theta)
    /// </summary>
    public (double x, double y, double theta)? GetRobotPose2D()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;

            return _navigationClient.GetRobotPose2D();
        }
    }

    /// <summary>
    /// Get the robot's current twist
    /// </summary>
    public (double x, double y, double theta, string frameId)? GetTwist()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;

            return _navigationClient.GetTwist();
        }
    }

    /// <summary>
    /// Get navigation feedback
    /// </summary>
    public NavigationFeedbackData? GetFeedback()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;

            return _navigationClient.GetFeedback();
        }
    }

    /// <summary>
    /// Get global planner path data from navigation_get_global_data.
    /// </summary>
    public GlobalPathData? GetGlobalPathData()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;

            return _navigationClient.GetGlobalPathData();
        }
    }

    /// <summary>
    /// Get local planner path data from navigation_get_local_data.
    /// </summary>
    public GlobalPathData? GetLocalPathData()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;

            return _navigationClient.GetLocalPathData();
        }
    }

    /// <summary>
    /// Get local planner cost map data from navigation_get_local_data.
    /// </summary>
    public CostMapData? GetCostMapData()
    {
        lock (_lock)
        {
            if (!_isInitialized || _navigationClient == null)
                return null;

            return _navigationClient.GetCostMapData();
        }
    }

    /// <summary>
    /// Get the configured robot footprint polygon (relative to base_link frame).
    /// </summary>
    public FootprintPoint[]? GetRobotFootprint()
    {
        return _config.RobotFootprint;
    }

    #endregion

    #region Dispatch Loops
    /// <summary>
    /// Odometry dispatch loop - runs in background task
    /// </summary>
    private async Task OdometryDispatchLoopAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("[ODOM-DISPATCH-LOOP] Starting odometry dispatch loop");
        
        while (_config.EnableOdometryDispatch && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _navigationClient == null || _odometryService == null || !_navigationClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                continue;
            }
            try
            {   
                var odom = _odometryService.CurrentOdometry;
                if (!string.IsNullOrEmpty(odom.Header.FrameId))  // Check if odometry is valid
                {  if (_navigationClient != null)
                        {
                            Odometry navOdom = default;
                            try
                            {
                                // Convert Shared.Sensor.Odometry to NavigationInterop.Odometry
                                navOdom = odom.ToNavigationOdometry();
                                
                                // Dispatch the struct
                                _navigationClient.DispatchOdometry(navOdom, "/odom");
                            }
                            finally
                            {
                                
                                // Free allocated memory after dispatch (even if exception occurs)
                                NavigationConversionExtensions.FreeNavigationOdometry(ref navOdom);
                            }
                        }
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "[ODOM-DISPATCH-LOOP] Error in odometry dispatch loop");
            }

            await Task.Delay(_config.OdomDispatchIntervalMs, cancellationToken);
        }
        
        _logger.LogInformation("[ODOM-DISPATCH-LOOP] Odometry dispatch loop ended");
    }

    /// <summary>
    /// Laser scan dispatch loop - runs in background task
    /// </summary>
    private async Task LaserScan1DispatchLoopAsync(CancellationToken cancellationToken)
    {
        while (_config.EnableLaserScan1 && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _navigationClient == null || _lidarDevice1 == null || !_navigationClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                continue;
            }
            try
            {
                var scan = _lidarDevice1.CurrentMeasurementData;
                if (scan.HasValue)
                {
                    lock (_lock)
                    {
                        LaserScan updateLaserScan = default;
                        try
                        {
                            var originalScan = scan.Value;
                            
                            // Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
                            var timeSpan = originalScan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
                            Header scanHeader = new Header
                            {
                                seq = originalScan.Header.Seq,
                                sec = (uint)timeSpan.TotalSeconds,
                                nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
                                frame_id = Marshal.StringToHGlobalAnsi(originalScan.Header.FrameId ?? "fscan")
                            };
                            
                            // Allocate and copy ranges array
                            IntPtr rangesPtr = IntPtr.Zero;
                            nuint rangesCount = 0;
                            if (originalScan.Ranges != null && originalScan.Ranges.Length > 0)
                            {
                                float[] rangesFloat = Array.ConvertAll(originalScan.Ranges, value => (float)value);
                                int rangesSize = rangesFloat.Length * sizeof(float);
                                rangesPtr = Marshal.AllocHGlobal(rangesSize);
                                Marshal.Copy(rangesFloat, 0, rangesPtr, rangesFloat.Length);
                                rangesCount = (nuint)rangesFloat.Length;
                            }
                            
                            // Allocate and copy intensities array (only if same length as ranges to avoid native assumptions)
                            IntPtr intensitiesPtr = IntPtr.Zero;
                            nuint intensitiesCount = 0;
                            if (originalScan.Intensities != null && originalScan.Intensities.Length > 0 &&
                                originalScan.Ranges != null && originalScan.Intensities.Length == originalScan.Ranges.Length)
                            {
                                float[] intensitiesFloat = Array.ConvertAll(originalScan.Intensities, value => (float)value);
                                int intensitiesSize = intensitiesFloat.Length * sizeof(float);
                                intensitiesPtr = Marshal.AllocHGlobal(intensitiesSize);
                                Marshal.Copy(intensitiesFloat, 0, intensitiesPtr, intensitiesFloat.Length);
                                intensitiesCount = (nuint)intensitiesFloat.Length;
                            }
                            
                            updateLaserScan = new LaserScan
                            {
                                header = scanHeader,
                                angle_min = (float)-2.356194496154785,
                                angle_max = (float)2.3557233810424805,
                                angle_increment = (float)0.00581718236207962,
                                time_increment = (float)6.172839493956417e-05,
                                scan_time = (float)0.06666667014360428,
                                range_min = (float)0.0,
                                range_max = (float)100.0,
                                ranges = rangesPtr,
                                ranges_count = rangesCount,
                                intensities = intensitiesPtr,
                                intensities_count = intensitiesCount
                            };
                            // _logger.LogInformation("Start dispatching laser scan 1");
                            _navigationClient.DispatchLaserScan(updateLaserScan,"/fscan");
                        }
                        finally
                        {
                            // Free allocated memory after dispatch (even if exception occurs)
                            NavigationConversionExtensions.FreeNavigationLaserScan(ref updateLaserScan);
                        }
                    }
                }
            }
            catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
            {
                _logger.LogDebug("Laser scan dispatch loop stopped: {Message}", ex.Message);
                break;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in laser scan dispatch loop");
            }

            await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
        }
    }

    /// <summary>
    /// Laser scan 2 dispatch loop - runs in background task
    /// </summary>
    private async Task LaserScan2DispatchLoopAsync(CancellationToken cancellationToken)
    {
        while (_config.EnableLaserScan2 && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _navigationClient == null || _lidarDevice2 == null || !_navigationClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                continue;
            }

            try
            {
                var scan = _lidarDevice2.CurrentMeasurementData;
                if (scan.HasValue)
                {
                    lock (_lock)
                    {
                        if (_navigationClient != null)
                        {
                            LaserScan updateLaserScan = default;
                            try
                            {
                                var originalScan = scan.Value;
                                
                                // Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
                                var timeSpan = originalScan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
                                Header scanHeader = new Header
                                {
                                    seq = originalScan.Header.Seq,
                                    sec = (uint)timeSpan.TotalSeconds,
                                    nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
                                    frame_id = Marshal.StringToHGlobalAnsi(originalScan.Header.FrameId ?? "b_scan")
                                };
                                
                                // Allocate and copy ranges array
                                IntPtr rangesPtr = IntPtr.Zero;
                                nuint rangesCount = 0;
                                if (originalScan.Ranges != null && originalScan.Ranges.Length > 0)
                                {
                                    float[] rangesFloat = Array.ConvertAll(originalScan.Ranges, value => (float)value);
                                    int rangesSize = rangesFloat.Length * sizeof(float);
                                    rangesPtr = Marshal.AllocHGlobal(rangesSize);
                                    Marshal.Copy(rangesFloat, 0, rangesPtr, rangesFloat.Length);
                                    rangesCount = (nuint)rangesFloat.Length;
                                }
                                
                                // Allocate and copy intensities array (only if same length as ranges to avoid native assumptions)
                                IntPtr intensitiesPtr = IntPtr.Zero;
                                nuint intensitiesCount = 0;
                                if (originalScan.Intensities != null && originalScan.Intensities.Length > 0 &&
                                    originalScan.Ranges != null && originalScan.Intensities.Length == originalScan.Ranges.Length)
                                {
                                    float[] intensitiesFloat = Array.ConvertAll(originalScan.Intensities, value => (float)value);
                                    int intensitiesSize = intensitiesFloat.Length * sizeof(float);
                                    intensitiesPtr = Marshal.AllocHGlobal(intensitiesSize);
                                    Marshal.Copy(intensitiesFloat, 0, intensitiesPtr, intensitiesFloat.Length);
                                    intensitiesCount = (nuint)intensitiesFloat.Length;
                                }
                                
                                updateLaserScan = new LaserScan
                                {
                                    header = scanHeader,
                                    angle_min = (float)-1.0707963705062866,
                                    angle_max = (float)1.0707963705062866,
                                    angle_increment = (float)0.00581718236207962,
                                    time_increment = (float)6.172839493956417e-05,
                                    scan_time = (float)0.06666667014360428,
                                    range_min = (float)0.0,
                                    range_max = (float)100.0,
                                    ranges = rangesPtr,
                                    ranges_count = rangesCount,
                                    intensities = intensitiesPtr,
                                    intensities_count = intensitiesCount
                                };
                                _navigationClient.DispatchLaserScan(updateLaserScan, "/b_scan");
                            }
                            finally
                            {
                                // Free allocated memory after dispatch (even if exception occurs)
                                NavigationConversionExtensions.FreeNavigationLaserScan(ref updateLaserScan);
                            }
                        }
                    }
                }
            }
            catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
            {
                _logger.LogDebug("Laser scan 2 dispatch loop stopped: {Message}", ex.Message);
                break;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in laser scan 2 dispatch loop");
            }

            await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
        }
    }

    /// <summary>
    /// Laser scan 3 dispatch loop - runs in background task
    /// </summary>
    private async Task LaserScan3DispatchLoopAsync(CancellationToken cancellationToken)
    {
        while (_config.EnableLaserScan3 && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _navigationClient == null || _lidarDevice3 == null || !_navigationClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                continue;
            }
            try
            {
                var scan = _lidarDevice3.CurrentMeasurementData;
                if (scan.HasValue)
                {
                    lock (_lock)
                    {
                        if (_navigationClient != null)
                        {
                            LaserScan updateLaserScan = default;
                            try
                            {
                                var originalScan = scan.Value;
                                
                                // Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
                                var timeSpan = originalScan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
                                Header scanHeader = new Header
                                {
                                    seq = originalScan.Header.Seq,
                                    sec = (uint)timeSpan.TotalSeconds,
                                    nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
                                    frame_id = Marshal.StringToHGlobalAnsi(originalScan.Header.FrameId ?? "f_scan")
                                };
                                
                                // Allocate and copy ranges array
                                IntPtr rangesPtr = IntPtr.Zero;
                                nuint rangesCount = 0;
                                if (originalScan.Ranges != null && originalScan.Ranges.Length > 0)
                                {
                                    float[] rangesFloat = Array.ConvertAll(originalScan.Ranges, value => (float)value);
                                    int rangesSize = rangesFloat.Length * sizeof(float);
                                    rangesPtr = Marshal.AllocHGlobal(rangesSize);
                                    Marshal.Copy(rangesFloat, 0, rangesPtr, rangesFloat.Length);
                                    rangesCount = (nuint)rangesFloat.Length;
                                }
                                
                                // Allocate and copy intensities array (only if same length as ranges to avoid native assumptions)
                                IntPtr intensitiesPtr = IntPtr.Zero;
                                nuint intensitiesCount = 0;
                                if (originalScan.Intensities != null && originalScan.Intensities.Length > 0 &&
                                    originalScan.Ranges != null && originalScan.Intensities.Length == originalScan.Ranges.Length)
                                {
                                    float[] intensitiesFloat = Array.ConvertAll(originalScan.Intensities, value => (float)value);
                                    int intensitiesSize = intensitiesFloat.Length * sizeof(float);
                                    intensitiesPtr = Marshal.AllocHGlobal(intensitiesSize);
                                    Marshal.Copy(intensitiesFloat, 0, intensitiesPtr, intensitiesFloat.Length);
                                    intensitiesCount = (nuint)intensitiesFloat.Length;
                                }
                                
                                updateLaserScan = new LaserScan
                                {
                                    header = scanHeader,
                                    angle_min = (float)-1.0707963705062866,
                                    angle_max = (float)1.0707963705062866,
                                    angle_increment = (float)0.00581718236207962,
                                    time_increment = (float)6.172839493956417e-05,
                                    scan_time = (float)0.06666667014360428,
                                    range_min = (float)0.0,
                                    range_max = (float)100.0,
                                    ranges = rangesPtr,
                                    ranges_count = rangesCount,
                                    intensities = intensitiesPtr,
                                    intensities_count = intensitiesCount
                                };
                                _navigationClient.DispatchLaserScan(updateLaserScan, "/f_scan");
                            }
                            finally
                            {
                                // Free allocated memory after dispatch (even if exception occurs)
                                NavigationConversionExtensions.FreeNavigationLaserScan(ref updateLaserScan);
                            }
                        }
                    }
                }
            }
            catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
            {
                _logger.LogDebug("Laser scan 3 dispatch loop stopped: {Message}", ex.Message);
                break;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in laser scan 3 dispatch loop");
            }

            await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
        }
    }

    /// <summary>
    /// Dispatch grid map once during initialization
    /// </summary>
    private async Task DispatchGridMapOnceAsync(CancellationToken cancellationToken)
    {
        // Wait for navigation to be ready
        int readyCheckCount = 0;
        const int maxReadyChecks = 50; // Wait up to 5 seconds (50 * 100ms)
        
        while (readyCheckCount < maxReadyChecks && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _navigationClient == null || _xlocService == null || !_navigationClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                readyCheckCount++;
                continue;
            }

            // Try to dispatch the map
            if (TryDispatchGridMap())
            {
                _logger.LogInformation("[GRIDMAP-DISPATCH] Grid map dispatched successfully during initialization");
                return;
            }

            await Task.Delay(100, cancellationToken);
            readyCheckCount++;
        }

        if (readyCheckCount >= maxReadyChecks)
        {
            _logger.LogWarning("[GRIDMAP-DISPATCH] Failed to dispatch grid map during initialization after {Count} attempts", maxReadyChecks);
        }
    }

    /// <summary>
    /// Check if map has changed and dispatch if needed
    /// Only handles static map from xloc
    /// </summary>
    private void CheckAndDispatchGridMapIfChanged()
    {
        if (!_isInitialized || _navigationClient == null || _xlocService == null || !_navigationClient.IsInitialized)
            return;

        try
        {
            var diagnostics = _xlocService.GetDiagnostics();
            if (diagnostics == null)
                return;

            // Check if active static map changed
            string currentMap = diagnostics.CurrentActiveMap ?? string.Empty;
            if (currentMap != _lastActiveMapName)
            {
                _logger.LogInformation("[GRIDMAP-DISPATCH] Active map changed from '{OldMap}' to '{NewMap}', dispatching new map", 
                    _lastActiveMapName ?? "(none)", currentMap);
                _lastActiveMapName = currentMap;
                
                if (!string.IsNullOrEmpty(currentMap))
                {
                    TryDispatchGridMap();
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "[GRIDMAP-DISPATCH] Error checking for map changes");
        }
    }

    /// <summary>
    /// Try to dispatch static grid map - returns true if successful
    /// Only handles static map from xloc
    /// </summary>
    private bool TryDispatchGridMap()
    {
        if (!_isInitialized || _navigationClient == null || _xlocService == null || !_navigationClient.IsInitialized)
            return false;

        try
        {
            // Check xloc diagnostics to ensure it's ready
            var diagnostics = _xlocService.GetDiagnostics();
            if (diagnostics == null)
                return false;

            // Check if there's an active static map
            if (string.IsNullOrEmpty(diagnostics.CurrentActiveMap))
            {
                return false;
            }

            // Get static gridmap data from xloc
            OccupancyGridData? gridData = _xlocService.GetStaticGridMap(reloadFromFile: true);

            if (gridData == null)
            {
                return false;
            }

            // Validate grid data before processing
            if (gridData.Width == 0 || gridData.Height == 0)
            {
                _logger.LogWarning("[GRIDMAP-DISPATCH] Invalid grid dimensions: width={Width}, height={Height}. Skipping dispatch.", 
                    gridData.Width, gridData.Height);
                return false;
            }

            // Additional validation: check if resolution is reasonable
            if (gridData.Resolution <= 0 || gridData.Resolution > 1.0)
            {
                _logger.LogWarning("[GRIDMAP-DISPATCH] Invalid resolution: {Resolution}. Skipping dispatch.", gridData.Resolution);
                return false;
            }

            // Convert OccupancyGridData to xloc_occupancy_grid_t
            xloc_occupancy_grid_t xlocGrid = new xloc_occupancy_grid_t
            {
                header = new xloc_header_t
                {
                    seq = 0,
                    stamp = new xloc_unix_time_t { sec = 0, nsec = 0 },
                    frame_id = Marshal.StringToHGlobalAnsi(gridData.FrameId ?? "map")
                },
                resolution = gridData.Resolution,
                width = gridData.Width,
                height = gridData.Height,
                origin = new xloc_pose_t
                {
                    position = new double[] { gridData.Origin.X, gridData.Origin.Y, gridData.Origin.Z },
                    orientation = new double[] { gridData.Origin.Qx, gridData.Origin.Qy, gridData.Origin.Qz, gridData.Origin.Qw }
                },
                data = IntPtr.Zero,
                data_length = (nuint)(gridData.Data?.Length ?? 0)
            };

            // Allocate and copy data
            IntPtr dataPtr = IntPtr.Zero;
            try
            {
                if (gridData.Data != null && gridData.Data.Length > 0)
                {
                    dataPtr = Marshal.AllocHGlobal(gridData.Data.Length);
                    Marshal.Copy(gridData.Data, 0, dataPtr, gridData.Data.Length);
                    xlocGrid.data = dataPtr;
                }

                // Convert to Navigation OccupancyGrid
                OccupancyGrid navGrid = default;
                try
                {
                    navGrid = xlocGrid.ToNavigationOccupancyGrid();
                    
                    _logger.LogInformation("[GRIDMAP-DISPATCH] Dispatching grid map: width={Width}, height={Height}, resolution={Resolution}, data_count={DataCount}",
                        navGrid.info.width, navGrid.info.height, navGrid.info.resolution, 
                        navGrid.data_count.ToUInt64());
                    
                    // Dispatch the struct
                    lock (_lock)
                    {
                        if (_navigationClient != null)
                        {
                            _navigationClient.DispatchGridMap(navGrid, "/map");
                            return true;
                        }
                    }
                }
                finally
                {
                    // Free allocated memory after dispatch
                    NavigationConversionExtensions.FreeNavigationOccupancyGrid(ref navGrid);
                }
            }
            finally
            {
                // Free xloc grid data pointer
                if (dataPtr != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(dataPtr);
                }
                // Free frame_id string
                if (xlocGrid.header.frame_id != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(xlocGrid.header.frame_id);
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "[GRIDMAP-DISPATCH] Error dispatching grid map");
        }

        return false;
    }

    /// <summary>
    /// Twist linear dispatch loop - runs in background task
    /// </summary>
    private async Task TwistLinearDispatchLoopAsync(CancellationToken cancellationToken)
    {
        while (_config.EnableTwistLinear && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _navigationClient == null || _odometryService == null || !_navigationClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                continue;
            }
            try
            {
                var odom = _odometryService.CurrentOdometry;
                if (odom.Header.FrameId != null)
                {
                    // Extract linear velocities from odometry twist
                    // TwistWithCovariance -> Twist -> Linear -> Vector3 (X, Y, Z)
                    double linearX = odom.Twist.Twist.Linear.X;
                    double linearY = odom.Twist.Twist.Linear.Y;
                    double linearZ = odom.Twist.Twist.Linear.Z;

                    lock (_lock)
                    {
                        if (_navigationClient != null)
                        {
                            // Console.WriteLine($"Dispatching twist linear: x={linearX:F2}, y={linearY:F2}, z={linearZ:F2}");
                            _navigationClient.SetTwistLinear(linearX, linearY, linearZ);
                        }
                    }
                }
            }
            catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
            {
                _logger.LogDebug("Twist linear dispatch loop stopped: {Message}", ex.Message);
                break;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in twist linear dispatch loop");
            }

            await Task.Delay(_config.TwistDispatchIntervalMs, cancellationToken);
        }
    }

    /// <summary>
    /// Navigation twist dispatch loop - reads twist from navigation and sends to DifferentialDrive
    /// Only sends when PS5 controller is disabled to ensure only one source controls the robot
    /// </summary>
    private async Task NavigationTwistDispatchLoopAsync(CancellationToken cancellationToken)
    {
        while (_config.EnableNavigationTwistDispatch && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _navigationClient == null || _inverseKinematics == null || !_navigationClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                continue;
            }
            // Check if PS5 controller is disabled - only send navigation twist when PS5 is disabled
            if (_ps5ControllerService != null && _ps5ControllerService.State == PS5ControllerState.Active)
            {
                // PS5 is active, skip navigation twist to avoid conflict
                await Task.Delay(_config.NavigationTwistDispatchIntervalMs, cancellationToken);
                continue;
            }

            try
            {
                // Read twist from navigation
                // Console.WriteLine("Checking for navigation twist...");
                var twistResult = _navigationClient.GetTwist();
                if (twistResult.HasValue)
                {
                    var (x, y, theta, frameId) = twistResult.Value;
                    // Console.WriteLine($"Got navigation twist: x={x:F2}, y={y:F2}, theta={theta:F2}, frameId={frameId}");
                    // Convert Twist2D to Twist (Shared.Geometry.Twist)
                    var twist = new RobotNet10.Shared.Geometry.Twist
                    {
                        Linear = new RobotNet10.Shared.Geometry.Vector3(x, 0.0, 0.0),
                        Angular = new RobotNet10.Shared.Geometry.Vector3(0.0, 0.0, theta)
                    };

                    // Send to DifferentialDrive - use semaphore to ensure only one thread sends at a time
                    // Check PS5 state before acquiring semaphore to avoid unnecessary blocking
                    if (_ps5ControllerService != null && _ps5ControllerService.State == PS5ControllerState.Active)
                    {
                        // PS5 became active, skip this iteration
                        continue;
                    }

                    // Acquire semaphore to ensure only one thread sends twist at a time
                    if (await _twistDispatchSemaphore.WaitAsync(0, cancellationToken))
                    {
                        try
                        {
                            // Double-check PS5 is still disabled and inverse kinematics is available
                            if (_inverseKinematics != null)
                            {
                                // Console.WriteLine("Sending navigation twist to DifferentialDrive...");
                                _inverseKinematics.Enable();
                                await _inverseKinematics.SetVelocityAsync(twist, cancellationToken);
                            }
                        }
                        catch (Exception ex)
                        {
                            _logger.LogError(ex, "Error sending navigation twist to DifferentialDrive");
                        }
                        finally
                        {
                            _twistDispatchSemaphore.Release();
                        }
                    }
                }
            }
            catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
            {
                _logger.LogDebug("Navigation twist dispatch loop stopped: {Message}", ex.Message);
                break;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error in navigation twist dispatch loop");
            }

            await Task.Delay(_config.NavigationTwistDispatchIntervalMs, cancellationToken);
        }
    }

    #endregion

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

        _disposed = true;

        try
        {
            // Stop update loop first
            lock (_lock)
            {
                _isInitialized = false;
            }
            
            // Give loop time to stop
            Thread.Sleep(500);
            
            _updateTimer?.Dispose();
            _navigationClient?.Dispose();
            _twistDispatchSemaphore?.Dispose();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing NavigationIntegrationService");
        }
    }
}

/// <summary>
/// Configuration for NavigationIntegrationService
/// </summary>
public class NavigationIntegrationConfiguration
{
    /// <summary>
    /// Enable/disable the integration service
    /// </summary>
    public bool Enabled { get; set; } = false;

    /// <summary>
    /// Update interval in milliseconds for periodic state updates (default: 100ms = 10Hz)
    /// </summary>
    public int UpdateIntervalMs { get; set; } = 100;

    /// <summary>
    /// Enable integration with NavigationClient for pose updates
    /// </summary>
    public bool EnableNavigationIntegration { get; set; } = true;

    /// <summary>
    /// Robot footprint points (polygon outline)
    /// If null or empty, default footprint will be used
    /// </summary>
    public FootprintPoint[]? RobotFootprint { get; set; } = null;

    /// <summary>
    /// Enable adding static map from Xloc during initialization
    /// </summary>
    public bool EnableStaticMap { get; set; } = true;

    /// <summary>
    /// Enable odometry dispatch loop
    /// </summary>
    public bool EnableOdometryDispatch { get; set; } = true;

    /// <summary>
    /// Odometry dispatch interval in milliseconds (default: 20ms = 50Hz)
    /// </summary>
    public int OdomDispatchIntervalMs { get; set; } = 20;

    /// <summary>
    /// Enable laser scan dispatch loop
    /// </summary>
    public bool EnableLaserScan1 { get; set; } = true;

    /// <summary>
    /// Enable static grid map dispatch from xloc.
    /// Map is dispatched once during initialization and when the active map changes.
    /// </summary>
    public bool EnableGridMapDispatch { get; set; } = false;

    /// <summary>
    /// [DEPRECATED] GridMap dispatch interval - no longer used.
    /// Map is now dispatched only when needed (initialization and map changes).
    /// Kept for backward compatibility.
    /// </summary>
    public int GridMapDispatchIntervalMs { get; set; } = 1000;

    /// <summary>
    /// [DEPRECATED] Navigation only uses static grid map from xloc.
    /// This property is kept for backward compatibility but is not used.
    /// </summary>
    public bool UseOnlineGridMap { get; set; } = false;

    /// <summary>
    /// LIDAR device ID for laser scan dispatch (Lidar 1)
    /// </summary>
    public string Lidar1DeviceId { get; set; } = string.Empty;

    /// <summary>
    /// Enable laser scan 2 dispatch loop
    /// </summary>
    public bool EnableLaserScan2 { get; set; } = false;

    /// <summary>
    /// LIDAR 2 device ID for laser scan dispatch (right side)
    /// </summary>
    public string Lidar2DeviceId { get; set; } = string.Empty;

    /// <summary>
    /// Enable laser scan 3 dispatch loop
    /// </summary>
    public bool EnableLaserScan3 { get; set; } = false;

    /// <summary>
    /// LIDAR 3 device ID for laser scan dispatch (left side)
    /// </summary>
    public string Lidar3DeviceId { get; set; } = string.Empty;

    /// <summary>
    /// Laser scan dispatch interval in milliseconds (default: 50ms = 20Hz)
    /// </summary>
    public int LaserScanDispatchIntervalMs { get; set; } = 50;

    /// <summary>
    /// Enable twist linear dispatch loop (from odometry)
    /// </summary>
    public bool EnableTwistLinear { get; set; } = true;

    /// <summary>
    /// Twist dispatch interval in milliseconds (default: 20ms = 50Hz)
    /// </summary>
    public int TwistDispatchIntervalMs { get; set; } = 20;

    /// <summary>
    /// Enable navigation twist dispatch loop (reads twist from navigation and sends to DifferentialDrive)
    /// Only sends when PS5 controller is disabled
    /// </summary>
    public bool EnableNavigationTwistDispatch { get; set; } = true;

    /// <summary>
    /// Navigation twist dispatch interval in milliseconds (default: 20ms = 50Hz)
    /// </summary>
    public int NavigationTwistDispatchIntervalMs { get; set; } = 20;
}

/// <summary>
/// Robot footprint point configuration
/// </summary>
public class FootprintPoint
{
    public double X { get; set; }
    public double Y { get; set; }
    public double Z { get; set; }
}