using Microsoft.AspNetCore.SignalR;
using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Hubs;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.TF3;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Sensor;
using System;
using System.Collections.Concurrent;
using System.Threading;
using System.Threading.Tasks;
// using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.Xloc;

/// <summary>
/// Integration service to dispatch sensor data from RobotNet to XLOC SLAM engine
/// Sends EKF filtered odometry, IMU raw data, and LIDAR scans
/// </summary>
public class XlocIntegrationService : IHostedService, IDisposable
{
    private readonly XlocIntegrationConfiguration _config;
    private readonly IDeviceProvider _deviceProvider;
    private readonly OdometryService? _odometryService;
    private readonly EKFService? _ekfService;
    private readonly ILogger<XlocIntegrationService> _logger;
    private readonly IHubContext<XlocPoseHub>? _poseHubContext;
    private readonly TF3BufferManager? _tf3BufferManager;
    private readonly object _lock = new();
    private readonly object _statsLock = new();  // Lock for thread-safe statistics access
    private readonly object _timestampLock = new();  // Lock for monotonic timestamp adjustment
    private readonly object _sourceSampleLock = new();  // Lock for source sample deduplication
    private readonly ConcurrentDictionary<string, DateTime> _latencyLogTimes = new();
    private readonly ConcurrentDictionary<string, DateTime> _lastDispatchedTimestamps = new();
    private readonly ConcurrentDictionary<string, DateTime> _stampLogTimes = new();
    private readonly ConcurrentDictionary<string, double> _lastSourceTimestamps = new();
    private readonly ConcurrentDictionary<string, uint> _lastSourceSequences = new();
    private readonly SemaphoreSlim _dispatchSemaphore = new SemaphoreSlim(1, 1);  // Only 1 dispatch at a time!

    private XlocClient? _xlocClient;
    private XlocAsyncDispatcher? _asyncDispatcher;

    // Separate timers for each sensor type (multi-threading)
    private Timer? _imuTimer;
    private Timer? _odomTimer;
    private Timer? _lidarTimer;
    private Timer? _loggingTimer;  // Timer for console logging of pose and diagnostics

    private bool _disposed = false;
    private bool _isInitialized = false;

    /// <summary>
    /// UTC time when <see cref="StopLocalization"/> last succeeded (API or direct). Used by
    /// <see cref="XlocAutoLocalizationHostedService"/> to avoid immediately re-starting after user stop.
    /// </summary>
    public DateTime? LastLocalizationStopUtc { get; private set; }

    // Event handler references (stored for unregistration)
    private EventHandler<LidarScanDataEventArgs>? _lidar1ScanHandler;
    private EventHandler<LidarScanDataEventArgs>? _lidar2ScanHandler;
    private EventHandler<LidarScanDataEventArgs>? _lidar3ScanHandler;
    private EventHandler<OdometryUpdatedEventArgs>? _odomUpdatedHandler;
    private EventHandler<AngularVelocityChangedEventArgs>? _imuAngularVelocityHandler;

    // Lidar event pump queue system
    private readonly ConcurrentQueue<(int slot, LaserScan scan)> _pendingLidarQueue1 = new();
    private readonly ConcurrentQueue<(int slot, LaserScan scan)> _pendingLidarQueue2 = new();
    private readonly ConcurrentQueue<(int slot, LaserScan scan)> _pendingLidarQueue3 = new();
    private const int MaxPendingLidarScans = 20;
    private Task? _lidarEventPumpTask;
    private CancellationTokenSource? _lidarEventPumpCts;

    // Cached device references
    private IInertialMeasurementUnit? _imuDevice;
    private ILidar? _lidarDevice1;
    private ILidar? _lidarDevice2;  // Second lidar (right side)
    private ILidar? _lidarDevice3;  // Third lidar (left side)

    // Cached laser scans for web visualization (thread-safe)
    private LaserScan? _cachedLaserScan1 = null;
    private LaserScan? _cachedLaserScan2 = null;
    private LaserScan? _cachedLaserScan3 = null;
    private readonly object _laserCacheLock = new();

    // Statistics (accessed from multiple threads)
    private long _odometryCount = 0;
    private long _imuCount = 0;
    private long _lidarCount = 0;
    private long _lidar1DispatchCount = 0;
    private long _lidar2DispatchCount = 0;
    private long _lidar3DispatchCount = 0;
    private uint _imuSeq = 0;
    private DateTime _lastStatsLog = DateTime.UtcNow;

    // Frequency snapshot tracking
    private long _lastOdomCountSnapshot;
    private long _lastImuCountSnapshot;
    private long _lastLidar1CountSnapshot;
    private long _lastLidar2CountSnapshot;
    private long _lastLidar3CountSnapshot;
    private DateTime _lastFreqSnapshotTime = DateTime.UtcNow;

    public XlocIntegrationService(
        IConfiguration configuration,
        IDeviceProvider deviceProvider,
        ILogger<XlocIntegrationService> logger,
        TF3BufferManager? tf3BufferManager = null,
        IHubContext<XlocPoseHub>? poseHubContext = null,
        OdometryService? odometryService = null,
        EKFService? ekfService = null)
    {
        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
        _poseHubContext = poseHubContext;
        _odometryService = odometryService;
        _ekfService = ekfService;
        _tf3BufferManager = tf3BufferManager;

        // Load configuration
        var configSection = configuration.GetSection("Xloc:Integration");
        if (!configSection.Exists())
        {
            _logger.LogWarning("Configuration section 'Xloc:Integration' not found. Using defaults.");
            _config = new XlocIntegrationConfiguration();
        }
        else
        {
            _config = new XlocIntegrationConfiguration();
            configSection.Bind(_config);
        }

        ValidateConfiguration();
    }

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

            if (_config.EnableEKFOdometry && _ekfService == null)
            {
                _logger.LogWarning("EKF Odometry is enabled but EKFService is not available. Will be disabled.");
                _config.EnableEKFOdometry = false;
            }

            if (_config.EnableRawOdometry && _odometryService == null)
            {
                _logger.LogWarning("Raw Odometry is enabled but OdometryService is not available. Will be disabled.");
                _config.EnableRawOdometry = false;
            }
        }
    }

    /// <summary>
    /// Get the XlocClient instance for use in other services
    /// </summary>
    public XlocClient? XlocClient => _xlocClient;

    public async Task StartAsync(CancellationToken cancellationToken)
    {
        // var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
        if (!_config.Enabled)
        {
            _logger.LogInformation("XlocIntegrationService is disabled in configuration");
            return;
        }

        _logger.LogInformation("Starting XlocIntegrationService...");

        try
        {
            // Wait for devices to be connected
            _logger.LogInformation("Waiting for devices to be connected...");
            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);

            if (!connected)
            {
                _logger.LogWarning("Timeout waiting for devices. XlocIntegrationService will retry initialization.");
                _ = Task.Run(async () => await RetryInitializationAsync(cancellationToken));
                return;
            }


            await InitializeAsync(cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error starting XlocIntegrationService");
        }
    }

    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("Devices are now connected. Initializing XlocIntegrationService...");
                await InitializeAsync(cancellationToken);
                return;
            }

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

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

    private async Task InitializeAsync(CancellationToken cancellationToken)
    {
        lock (_lock)
        {
            if (_isInitialized)
                return;

            try
            {
                // Get device references
                if (_config.EnableIMU && !string.IsNullOrEmpty(_config.ImuDeviceId))
                {
                    var device = _deviceProvider.GetDevice(_config.ImuDeviceId);
                    if (device is IInertialMeasurementUnit imu && device.IsConnected)
                    {
                        _imuDevice = imu;
                        _logger.LogInformation("IMU device '{DeviceId}' found and connected", _config.ImuDeviceId);
                    }
                    else
                    {
                        _logger.LogWarning("IMU device '{DeviceId}' not found or not connected. IMU data will not be sent.",
                            _config.ImuDeviceId);
                    }
                }

                if (_config.EnableLidar1 && !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. LIDAR data will not be sent.",
                            _config.Lidar1DeviceId);
                    }
                }

                // Second lidar (right side)
                if (_config.EnableLidar2 && !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.",
                            _config.Lidar2DeviceId);
                    }
                }

                // Third lidar (left side)
                if (_config.EnableLidar3 && !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.",
                            _config.Lidar3DeviceId);
                    }
                }

                // Create and initialize xloc client with TF3 buffer from TF3BufferManager
                if (_tf3BufferManager == null || !_tf3BufferManager.IsInitialized)
                {
                    throw new InvalidOperationException(
                        "TF3BufferManager is not available or not initialized. " +
                        "Ensure TF3BufferManager is registered in dependency injection and initialized before XlocIntegrationService.");
                }
                
                IntPtr tfBuffer = _tf3BufferManager.TfBuffer;
                _logger.LogInformation("[XLOC] Using TF3 buffer from TF3BufferManager: {Buffer}", tfBuffer);
                
                _xlocClient = new XlocClient(tfBuffer, _logger);
                _xlocClient.Initialize();

                _logger.LogInformation("XlocClient initialized successfully");

                // DON'T start mapping/localization immediately - wait for sensor data first!
                // This prevents crash when xloc doesn't have any data yet
                
                if (_config.Mode == XlocMode.Localization && !string.IsNullOrEmpty(_config.MapFilePath))
                {
                    if (_xlocClient.ActivateMap(_config.MapFilePath))
                    {
                        _logger.LogInformation("Map activated: {MapFile}", _config.MapFilePath);
                    }
                    else
                    {
                        _logger.LogWarning("Failed to activate map. Will try to start without map.");
                    }
                }
                // Create async dispatcher — single-thread model (like ROS ros::spin)
                _asyncDispatcher = new XlocAsyncDispatcher(_xlocClient, queueCapacity: 300, logger: _logger);
                _logger.LogInformation("[XLOC] Async dispatcher created (single-thread, capacity=300)");

                // Verify timestamp conversion at startup
                VerifyTimestampConversion();

                // Register event-driven odometry dispatch
                if (_config.EnableRawOdometry && _odometryService != null)
                {
                    _odomUpdatedHandler = OnOdometryUpdated;
                    _odometryService.OdometryUpdated += _odomUpdatedHandler;
                    _logger.LogInformation("[XLOC] Odometry event handler registered (event-driven)");
                }

                // Register event-driven IMU dispatch
                if (_config.EnableIMU && _imuDevice != null)
                {
                    _imuAngularVelocityHandler = OnImuAngularVelocityChanged;
                    _imuDevice.AngularVelocityChanged += _imuAngularVelocityHandler;
                    _logger.LogInformation("[XLOC] IMU event handler registered (event-driven)");
                }

                // Register event-driven lidar dispatch with queue pump
                RegisterLidarEventDispatch(cancellationToken);

                // Start console logging timer (every 2 seconds)
                _loggingTimer = new Timer(LogPoseAndDiagnostics, null,
                    TimeSpan.FromSeconds(2), TimeSpan.FromSeconds(2));
                _logger.LogInformation("XLOC console logging started (every 2 seconds)");

                _isInitialized = true;
                _logger.LogInformation("XlocIntegrationService initialized successfully with event-driven sensor dispatch");
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Failed to initialize XlocIntegrationService");
                throw;
            }
        }

        await Task.CompletedTask;
    }// IMU message sequence counter

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

        try
        {
            // IMPORTANT: Set _isInitialized to false FIRST to signal event handlers to stop
            lock (_lock)
            {
                _isInitialized = false;
            }

            // Unregister all event handlers immediately
            UnregisterLidarEventDispatch();
            UnregisterOdomImuEventDispatch();

            // Dispose timers
            _imuTimer?.Dispose();
            _imuTimer = null;
            _odomTimer?.Dispose();
            _odomTimer = null;
            _lidarTimer?.Dispose();
            _lidarTimer = null;
            _loggingTimer?.Dispose();
            _loggingTimer = null;

            // Wait for in-flight dispatches to complete
            _logger.LogInformation("Waiting for in-flight dispatches to complete...");
            await Task.Delay(500, cancellationToken);

            // Dispose async dispatcher BEFORE stopping xloc client (prevent race conditions)
            var dispatcher = _asyncDispatcher;
            _asyncDispatcher = null;
            dispatcher?.Dispose();

            // Stop xloc
            if (_xlocClient != null)
            {
                if (_config.Mode == XlocMode.Localization)
                {
                    _xlocClient.StopLocalization();
                }
                else if (_config.Mode == XlocMode.Mapping && !string.IsNullOrEmpty(_config.SaveMapFilePath))
                {
                    _xlocClient.StopMapping(_config.SaveMapFilePath);
                    _logger.LogInformation("Map saved to: {MapFile}", _config.SaveMapFilePath);
                }

                _xlocClient.Dispose();
                _xlocClient = null;
                _logger.LogInformation("XlocClient disposed successfully");
            }

            LogFinalStatistics();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error stopping XlocIntegrationService");
        }

        await Task.CompletedTask;
    }

    #region Event-Driven Dispatch — Odometry, IMU, Lidar

    /// <summary>
    /// Event handler: called when OdometryService fires OdometryUpdated.
    /// Deduplicates, adjusts timestamp, and dispatches via async dispatcher.
    /// </summary>
    private void OnOdometryUpdated(object? sender, OdometryUpdatedEventArgs e)
    {
        if (!_isInitialized || _xlocClient == null || _asyncDispatcher == null)
            return;

        try
        {
            var rawOdom = e.Odometry;
            if (rawOdom.Header.FrameId == null)
                return;

            // Dedup by sequence + timestamp
            var ts = (rawOdom.Header.Stamp - DateTime.UnixEpoch).TotalSeconds;
            if (!TryAcceptSourceSample(_config.RawOdometrySensorId, rawOdom.Header.Seq, ts))
                return;

            // Monotonic timestamp
            var stamp = GetAdjustedSensorStamp(_config.RawOdometrySensorId, DateTime.UtcNow);

            var updatedOdom = new Odometry
            {
                Header = new Header
                {
                    Seq = rawOdom.Header.Seq,
                    Stamp = stamp,
                    FrameId = rawOdom.Header.FrameId
                },
                ChildFrameId = rawOdom.ChildFrameId,
                Pose = rawOdom.Pose,
                Twist = rawOdom.Twist
            };

            var sensorId = _config.RawOdometrySensorId;
            var client = _xlocClient;
            var dispatcher = _asyncDispatcher;
            if (client == null || dispatcher == null) return;

            _ = dispatcher.EnqueueOdometryAsync(() =>
            {
                client.DispatchOdometry(updatedOdom, sensorId);
                Interlocked.Increment(ref _odometryCount);
            }, sensorId);
        }
        catch (TaskCanceledException) { }
        catch (OperationCanceledException) { }
        catch (InvalidOperationException ex) when (ex.Message.Contains("disposed"))
        {
            _logger.LogDebug("Odometry dispatch skipped: dispatcher disposed");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error in OnOdometryUpdated event handler");
        }
    }

    /// <summary>
    /// Event handler: called when IMU fires AngularVelocityChanged.
    /// Creates full IMU message, deduplicates, and dispatches via async dispatcher.
    /// </summary>
    private void OnImuAngularVelocityChanged(object? sender, AngularVelocityChangedEventArgs e)
    {
        if (!_isInitialized || _xlocClient == null || _imuDevice == null || _asyncDispatcher == null)
            return;

        try
        {
            var imu = CreateImuMessage(_imuDevice);
            if (imu == null)
                return;

            var originalImu = imu.Value;

            // Dedup by sequence + timestamp
            var ts = (originalImu.Header.Stamp - DateTime.UnixEpoch).TotalSeconds;
            if (!TryAcceptSourceSample(_config.ImuSensorId, originalImu.Header.Seq, ts))
                return;

            // Monotonic timestamp
            var stamp = GetAdjustedSensorStamp(_config.ImuSensorId, DateTime.UtcNow);

            var updatedImu = new Imu
            {
                Header = new Header
                {
                    Seq = originalImu.Header.Seq,
                    Stamp = stamp,
                    FrameId = originalImu.Header.FrameId
                },
                Orientation = originalImu.Orientation,
                OrientationCovariance = originalImu.OrientationCovariance,
                AngularVelocity = originalImu.AngularVelocity,
                AngularVelocityCovariance = originalImu.AngularVelocityCovariance,
                LinearAcceleration = originalImu.LinearAcceleration,
                LinearAccelerationCovariance = originalImu.LinearAccelerationCovariance
            };

            var sensorId = _config.ImuSensorId;
            var client = _xlocClient;
            var dispatcher = _asyncDispatcher;
            if (client == null || dispatcher == null) return;

            _ = dispatcher.EnqueueImuAsync(() =>
            {
                client.DispatchImu(updatedImu, sensorId);
                Interlocked.Increment(ref _imuCount);
            }, sensorId);
        }
        catch (TaskCanceledException) { }
        catch (OperationCanceledException) { }
        catch (InvalidOperationException ex) when (ex.Message.Contains("disposed"))
        {
            _logger.LogDebug("IMU dispatch skipped: dispatcher disposed");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error in OnImuAngularVelocityChanged event handler");
        }
    }

    #endregion

    #region Lidar Event Pump System

    /// <summary>
    /// Register lidar event handlers and start the pump loop.
    /// </summary>
    private void RegisterLidarEventDispatch(CancellationToken ct)
    {
        StartLidarEventPump(ct);

        if (_lidarDevice1 != null && _config.EnableLidar1)
        {
            _lidar1ScanHandler = (s, e) => QueuePendingLidarScan(1, e.MeasurementData);
            _lidarDevice1.ScanDataReceived += _lidar1ScanHandler;
            _logger.LogInformation("[XLOC] Lidar 1 event handler registered");
        }

        if (_lidarDevice2 != null && _config.EnableLidar2)
        {
            _lidar2ScanHandler = (s, e) => QueuePendingLidarScan(2, e.MeasurementData);
            _lidarDevice2.ScanDataReceived += _lidar2ScanHandler;
            _logger.LogInformation("[XLOC] Lidar 2 event handler registered");
        }

        if (_lidarDevice3 != null && _config.EnableLidar3)
        {
            _lidar3ScanHandler = (s, e) => QueuePendingLidarScan(3, e.MeasurementData);
            _lidarDevice3.ScanDataReceived += _lidar3ScanHandler;
            _logger.LogInformation("[XLOC] Lidar 3 event handler registered");
        }
    }

    /// <summary>
    /// Unregister lidar event handlers and stop the pump.
    /// </summary>
    private void UnregisterLidarEventDispatch()
    {
        if (_lidarDevice1 != null && _lidar1ScanHandler != null)
        {
            _lidarDevice1.ScanDataReceived -= _lidar1ScanHandler;
            _lidar1ScanHandler = null;
        }
        if (_lidarDevice2 != null && _lidar2ScanHandler != null)
        {
            _lidarDevice2.ScanDataReceived -= _lidar2ScanHandler;
            _lidar2ScanHandler = null;
        }
        if (_lidarDevice3 != null && _lidar3ScanHandler != null)
        {
            _lidarDevice3.ScanDataReceived -= _lidar3ScanHandler;
            _lidar3ScanHandler = null;
        }

        StopLidarEventPump();
    }

    /// <summary>
    /// Unregister odometry and IMU event handlers.
    /// </summary>
    private void UnregisterOdomImuEventDispatch()
    {
        if (_odometryService != null && _odomUpdatedHandler != null)
        {
            _odometryService.OdometryUpdated -= _odomUpdatedHandler;
            _odomUpdatedHandler = null;
        }
        if (_imuDevice != null && _imuAngularVelocityHandler != null)
        {
            _imuDevice.AngularVelocityChanged -= _imuAngularVelocityHandler;
            _imuAngularVelocityHandler = null;
        }
    }

    private void QueuePendingLidarScan(int slot, LaserScan scan)
    {
        var queue = slot switch
        {
            1 => _pendingLidarQueue1,
            2 => _pendingLidarQueue2,
            _ => _pendingLidarQueue3
        };

        var queueCountBefore = queue.Count;
        queue.Enqueue((slot, scan));

        // Trim queue to prevent unbounded growth
        int droppedCount = 0;
        while (queue.Count > MaxPendingLidarScans)
        {
            queue.TryDequeue(out _);
            droppedCount++;
        }

        if (droppedCount > 0)
        {
            // _logger.LogWarning("[XLOC-DIAG] scan_{Slot} QueuePending DROPPED {Dropped} scans (queue was {Before}, max={Max})",
            //     slot, droppedCount, queueCountBefore, MaxPendingLidarScans);
        }
    }

    private void StartLidarEventPump(CancellationToken externalCt)
    {
        _lidarEventPumpCts = CancellationTokenSource.CreateLinkedTokenSource(externalCt);
        _lidarEventPumpTask = Task.Run(() => LidarEventPumpLoopAsync(_lidarEventPumpCts.Token));
        _logger.LogInformation("[XLOC] Lidar event pump started");
    }

    private void StopLidarEventPump()
    {
        try
        {
            _lidarEventPumpCts?.Cancel();
            _lidarEventPumpTask?.Wait(TimeSpan.FromSeconds(2));
        }
        catch (AggregateException) { }
        catch (OperationCanceledException) { }
        finally
        {
            _lidarEventPumpCts?.Dispose();
            _lidarEventPumpCts = null;
            _lidarEventPumpTask = null;

            // Clear queues
            while (_pendingLidarQueue1.TryDequeue(out _)) { }
            while (_pendingLidarQueue2.TryDequeue(out _)) { }
            while (_pendingLidarQueue3.TryDequeue(out _)) { }
        }
    }

    private async Task LidarEventPumpLoopAsync(CancellationToken ct)
    {
        while (!ct.IsCancellationRequested)
        {
            try
            {
                var loopStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

                bool dispatchedAny = false;
                dispatchedAny |= await DrainLidarQueueAsync(_pendingLidarQueue1, "queue1", ct);
                dispatchedAny |= await DrainLidarQueueAsync(_pendingLidarQueue2, "queue2", ct);
                dispatchedAny |= await DrainLidarQueueAsync(_pendingLidarQueue3, "queue3", ct);

                var loopElapsed = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - loopStart;
                if (dispatchedAny && loopElapsed > 20)
                {
                    // _logger.LogWarning("[XLOC-DIAG] PumpLoop took {ElapsedMs}ms (q1={Q1},q2={Q2},q3={Q3})",
                    //     loopElapsed, _pendingLidarQueue1.Count, _pendingLidarQueue2.Count, _pendingLidarQueue3.Count);
                }

                if (!dispatchedAny)
                {
                    await Task.Delay(_config.LidarDispatchIntervalMs, ct);
                }
            }
            catch (OperationCanceledException) { break; }
            catch (Exception ex)
            {
                _logger.LogError(ex, "[XLOC] Error in lidar event pump loop");
            }
        }
    }

    private async Task<bool> DrainLidarQueueAsync(ConcurrentQueue<(int slot, LaserScan scan)> queue, string queueName, CancellationToken ct)
    {
        bool dispatched = false;
        int count = 0;
        var drainStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

        while (queue.TryDequeue(out var item) && !ct.IsCancellationRequested)
        {
            await DispatchLidarFromEventAsync(item.slot, item.scan);
            dispatched = true;
            count++;
        }

        if (dispatched)
        {
            var drainElapsed = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - drainStart;
            if (drainElapsed > 10)
            {
                // _logger.LogWarning("[XLOC-DIAG] Drain {Queue} took {ElapsedMs}ms for {Count} scans",
                //     queueName, drainElapsed, count);
            }
        }
        return dispatched;
    }

    /// <summary>
    /// Dispatch a lidar scan from event queue. Deduplicates, adjusts timestamp,
    /// caches for web visualization, and dispatches via async dispatcher.
    /// </summary>
    private async Task DispatchLidarFromEventAsync(int slot, LaserScan originalScan)
    {
        if (!_isInitialized || _xlocClient == null || _asyncDispatcher == null)
            return;

        try
        {
            var (sensorId, frameId) = slot switch
            {
                1 => (_config.Lidar1SensorId, "scan_1"),
                2 => (_config.Lidar2SensorId, "scan_2"),
                _ => (_config.Lidar3SensorId, "scan_3")
            };

            // Dedup
            var ts = (originalScan.Header.Stamp - DateTime.UnixEpoch).TotalSeconds;
            if (!TryAcceptSourceSample(sensorId, originalScan.Header.Seq, ts))
                return;

            // Monotonic timestamp
            var stamp = GetAdjustedSensorStamp(sensorId, DateTime.UtcNow);

            // Use sensor-provided angle parameters (QUYVN device config)
            // For Lidar 3, use hardcoded values as in original code
            var updatedScan = new LaserScan
            {
                Header = new Header
                {
                    Seq = originalScan.Header.Seq,
                    Stamp = stamp,
                    FrameId = frameId
                },
                AngleMin = slot == 3 ? (float)-2.356194496154785 : originalScan.AngleMin,
                AngleMax = slot == 3 ? (float)2.3557233810424805 : originalScan.AngleMax,
                AngleIncrement = slot == 3 ? (float)0.00581718236207962 : originalScan.AngleIncrement,
                TimeIncrement = slot == 3 ? (float)6.172839493956417e-05 : originalScan.TimeIncrement,
                ScanTime = slot == 3 ? (float)0.06666667014360428 : originalScan.ScanTime,
                RangeMin = slot == 3 ? 0f : originalScan.RangeMin,
                RangeMax = slot == 3 ? 100f : originalScan.RangeMax,
                Ranges = originalScan.Ranges,
                Intensities = originalScan.Intensities ?? []
            };

            // Always cache latest scan for web visualization even when xloc is stopped
            lock (_laserCacheLock)
            {
                switch (slot)
                {
                    case 1: _cachedLaserScan1 = updatedScan; break;
                    case 2: _cachedLaserScan2 = updatedScan; break;
                    case 3: _cachedLaserScan3 = updatedScan; break;
                }
            }

            // Dispatch to xloc via async dispatcher
            var client = _xlocClient;
            var dispatcher = _asyncDispatcher;
            if (client == null || dispatcher == null ) return;

            var lidarSlot = slot; // capture for lambda
            var enqueueTs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
            await dispatcher.EnqueueLaserScanAsync(() =>
            {
                var asyncWaitMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - enqueueTs;
                var nativeStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

                client.DispatchLaserScan(updatedScan, sensorId);

                var nativeElapsedMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - nativeStart;
                Interlocked.Increment(ref _lidarCount);
                switch (lidarSlot)
                {
                    case 1: Interlocked.Increment(ref _lidar1DispatchCount); break;
                    case 2: Interlocked.Increment(ref _lidar2DispatchCount); break;
                    case 3: Interlocked.Increment(ref _lidar3DispatchCount); break;
                }

                if (asyncWaitMs > 10 || nativeElapsedMs > 10)
                {
                    // _logger.LogWarning(
                    //     "[XLOC-DIAG] scan_{Slot} async_wait={AsyncWaitMs}ms native_call={NativeMs}ms (sensor={SensorId})",
                    //     lidarSlot, asyncWaitMs, nativeElapsedMs, sensorId);
                }
            }, sensorId);
        }
        catch (TaskCanceledException) { }
        catch (OperationCanceledException) { }
        catch (InvalidOperationException ex) when (ex.Message.Contains("disposed"))
        {
            _logger.LogDebug("Lidar {Slot} dispatch skipped: dispatcher disposed", slot);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error dispatching lidar {Slot} from event", slot);
        }
    }

    #endregion

    private Imu? CreateImuMessage(IInertialMeasurementUnit imu)
    {
        try
        {
            // Increment IMU sequence number
            var seq = Interlocked.Increment(ref _imuSeq);
            
            var header = new Header
            {
                Seq = seq,
                // Use device-provided timestamp directly (do not use MonotonicTimestampManager here)
                Stamp = imu.LastUpdateTime,
                FrameId = _config.ImuFrameId
            };

            // Get orientation (raw from IMU)
            var orientation = new Quaternion();
            var quatData = imu.CachedQuaternion;
            if (quatData.HasValue)
            {
                orientation = quatData.Value.Quaternion;
            }
            else
            {
                // Fallback to identity if no quaternion available
                orientation = new Quaternion { X = 0, Y = 0, Z = 0, W = 1 };
            }

            // Get angular velocity (raw from IMU)
            var angularVelocity = imu.CachedAngularVelocity.Vector;

            // Get linear acceleration (raw from IMU)
            var linearAcceleration = imu.CachedAcceleration.Accel.Linear;

            // ===== SEND RAW IMU DATA TO XLOC (NO TRANSFORMATION) =====
            // Testing to see if transformation is needed or causing issues

            // Covariance matrices (row-major, 3x3)
            // Based on Wheeltec N100 IMU specifications
            var orientationCov = new double[9] 
            { 
                0.01, 0.0, 0.0,   // Row 1: var_x, cov_xy, cov_xz
                0.0, 0.01, 0.0,   // Row 2: cov_yx, var_y, cov_yz
                0.0, 0.0, 0.01    // Row 3: cov_zx, cov_zy, var_z
            };
            
            var angularVelocityCov = new double[9]
            {
                0.01, 0.0, 0.0,
                0.0, 0.01, 0.0,
                0.0, 0.0, 0.01
            };
            
            var linearAccelerationCov = new double[9]
            {
                0.05, 0.0, 0.0,
                0.0, 0.05, 0.0,
                0.0, 0.0, 0.05
            };

            return new Imu
            {
                Header = header,
                Orientation = orientation,
                OrientationCovariance = orientationCov,
                AngularVelocity = angularVelocity,
                AngularVelocityCovariance = angularVelocityCov,
                LinearAcceleration = linearAcceleration,
                LinearAccelerationCovariance = linearAccelerationCov
            };
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error creating IMU message");
            return null;
        }
    }

    #region Helper Methods — Timestamp, Dedup, Delay

    /// <summary>
    /// Precise delay that compensates for drift instead of fixed Task.Delay.
    /// Resyncs if more than one full period late.
    /// Returns the updated nextCycleTime.
    /// </summary>
    private static async Task<DateTime> DelayUntilNextCycleAsync(int intervalMs, DateTime nextCycleTime, CancellationToken ct)
    {
        nextCycleTime = nextCycleTime.AddMilliseconds(intervalMs);
        var now = DateTime.UtcNow;
        var delay = nextCycleTime - now;

        if (delay.TotalMilliseconds < -intervalMs)
        {
            // More than one full period late — resync to avoid burst catch-up
            return now.AddMilliseconds(intervalMs);
        }

        if (delay > TimeSpan.Zero)
            await Task.Delay(delay, ct);

        return nextCycleTime;
    }

    /// <summary>
    /// Ensures strictly monotonic timestamps per sensor.
    /// If the new stamp equals or precedes the previous one, adds 1 tick.
    /// </summary>
    private DateTime GetAdjustedSensorStamp(string sensorId, DateTime rawStamp)
    {
        lock (_timestampLock)
        {
            // Fallback to UtcNow if stamp is default (uninitialized)
            if (rawStamp == default)
                rawStamp = DateTime.UtcNow;

            if (_lastDispatchedTimestamps.TryGetValue(sensorId, out var last) && rawStamp <= last)
            {
                rawStamp = last.AddTicks(1);
            }
            _lastDispatchedTimestamps[sensorId] = rawStamp;
            return rawStamp;
        }
    }

    /// <summary>
    /// Deduplicates source samples using dual strategy: sequence-based (preferred) + timestamp-based fallback.
    /// Returns false if the sample was already processed.
    /// When sequence is strictly increasing, use sequence alone.
    /// When sequence is unchanged (e.g. device always sends seq=0), fall through to timestamp-based dedup.
    /// </summary>
    private bool TryAcceptSourceSample(string sourceId, uint sequence, double timestamp)
    {
        lock (_sourceSampleLock)
        {
            bool hasLastSeq = _lastSourceSequences.TryGetValue(sourceId, out var lastSeq);

            if (hasLastSeq)
            {
                if (sequence > lastSeq)
                {
                    // Sequence strictly increased — accept without timestamp check
                    _lastSourceSequences[sourceId] = sequence;
                    _lastSourceTimestamps[sourceId] = timestamp;
                    return true;
                }

                if (sequence < lastSeq)
                {
                    // Sequence went backwards (duplicate or reorder) — reject
                    return false;
                }

                // sequence == lastSeq: device may not increment seq (e.g. always 0)
                // Fall through to timestamp-based dedup
            }

            // Timestamp-based dedup (primary when seq is unchanged, fallback otherwise)
            if (_lastSourceTimestamps.TryGetValue(sourceId, out var lastTs) && timestamp <= lastTs)
                return false;

            _lastSourceSequences[sourceId] = sequence;
            _lastSourceTimestamps[sourceId] = timestamp;
            return true;
        }
    }

    /// <summary>
    /// Validates DateTime to Unix time conversion roundtrip at startup.
    /// </summary>
    private void VerifyTimestampConversion()
    {
        var now = DateTime.UtcNow;
        var unixSec = (now - DateTime.UnixEpoch).TotalSeconds;
        var roundtrip = DateTime.UnixEpoch.AddSeconds(unixSec);
        var driftMs = Math.Abs((roundtrip - now).TotalMilliseconds);
        if (driftMs > 100)
            _logger.LogWarning("[XLOC] Timestamp conversion drift: {Drift:F3}ms — may cause issues", driftMs);
        else
            _logger.LogInformation("[XLOC] Timestamp conversion OK, drift: {Drift:F6}ms", driftMs);
    }

    #endregion

    private void LogStatistics()
    {
        _logger.LogInformation(
            "Xloc sensor data dispatched - Odometry: {OdomCount}, IMU: {ImuCount}, LIDAR: {LidarCount}",
            _odometryCount, _imuCount, _lidarCount);
    }

    private void LogFinalStatistics()
    {
        _logger.LogInformation(
            "XlocIntegrationService final statistics - Total dispatched: Odometry={OdomCount}, IMU={ImuCount}, LIDAR={LidarCount}",
            _odometryCount, _imuCount, _lidarCount);
    }

    /// <summary>
    /// Get current pose from xloc (thread-safe)
    /// </summary>
    public (double x, double y, double z, double qx, double qy, double qz, double qw)? GetCurrentPose()
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
                return null;

            return _xlocClient.GetCurrentPose();
        }
    }

    /// <summary>
    /// Get diagnostics from xloc (thread-safe)
    /// </summary>
    public XlocDiagnosticsData? GetDiagnostics()
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
                return null;

            return _xlocClient.GetDiagnostics();
        }
    }

    /// <summary>
    /// Get current pose as 2D (x, y, yaw) - easier to use for navigation
    /// </summary>
    public OccupancyGridData? GetStaticGridMap(bool reloadFromFile = false)
    {
        lock (_lock)
        {
            if (_xlocClient == null || !_isInitialized)
                return null;

            // Avoid calling native map getter before a map is active to prevent noisy native warnings.
            var diagnostics = _xlocClient.GetDiagnostics();
            if (diagnostics == null || string.IsNullOrWhiteSpace(diagnostics.CurrentActiveMap))
                return null;

            return _xlocClient.GetStaticGridMap(reloadFromFile);
        }
    }

    public OccupancyGridData? GetOnlineGridMap()
    {
        lock (_lock)
        {
            if (_xlocClient == null || !_isInitialized)
                return null;
            
            return _xlocClient.GetOnlineGridMap();
        }
    }

    /// <summary>
    /// Get static grid map pointer (from loaded map file)
    /// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
    /// </summary>
    public IntPtr GetStaticGridMapPtr(bool reloadFromFile = false)
    {
        lock (_lock)
        {
            if (_xlocClient == null || !_isInitialized)
                return IntPtr.Zero;
            
            return _xlocClient.GetStaticGridMapPtr(reloadFromFile);
        }
    }

    /// <summary>
    /// Get online grid map pointer (from SLAM)
    /// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
    /// </summary>
    public IntPtr GetOnlineGridMapPtr()
    {
        lock (_lock)
        {
            if (_xlocClient == null || !_isInitialized)
                return IntPtr.Zero;
            
            return _xlocClient.GetOnlineGridMapPtr();
        }
    }

    public (double x, double y, double yaw)? GetCurrentPose2D()
    {
        var pose = GetCurrentPose();
        if (!pose.HasValue)
            return null;

        var (x, y, z, qx, qy, qz, qw) = pose.Value;

        // Convert quaternion to yaw angle
        double yaw = Math.Atan2(2.0 * (qw * qz + qx * qy),
                                1.0 - 2.0 * (qy * qy + qz * qz));

        return (x, y, yaw);
    }

    /// <summary>
    /// Get sampled laser scan data for web visualization (1 degree intervals)
    /// Returns array of (angle, range) pairs
    /// Thread-safe
    /// </summary>
    public List<(float angle, float range)>? GetSampledLaserScan()
    {
        return GetSampledLaserScanInternal(_cachedLaserScan1, 360);
    }

    /// <summary>
    /// Get sampled laser scan data from Lidar 2 (right side)
    /// </summary>
    public List<(float angle, float range)>? GetSampledLaserScan2()
    {
        return GetSampledLaserScanInternal(_cachedLaserScan2, 360);
    }

    /// <summary>
    /// Get sampled laser scan data from Lidar 3 (left side)
    /// </summary>
    public List<(float angle, float range)>? GetSampledLaserScan3()
    {
        return GetSampledLaserScanInternal(_cachedLaserScan3, 360);
    }

    /// <summary>
    /// Get full laser scan data (no downsampling) from Lidar 1 for web visualization.
    /// </summary>
    public List<(float angle, float range)>? GetFullLaserScan()
    {
        return GetFullLaserScanInternal(_cachedLaserScan1);
    }

    /// <summary>
    /// Get full laser scan data (no downsampling) from Lidar 2.
    /// </summary>
    public List<(float angle, float range)>? GetFullLaserScan2()
    {
        return GetFullLaserScanInternal(_cachedLaserScan2);
    }

    /// <summary>
    /// Get full laser scan data (no downsampling) from Lidar 3.
    /// </summary>
    public List<(float angle, float range)>? GetFullLaserScan3()
    {
        return GetFullLaserScanInternal(_cachedLaserScan3);
    }

    /// <summary>
    /// Internal method to sample laser scan data
    /// </summary>
    private List<(float angle, float range)>? GetSampledLaserScanInternal(LaserScan? cachedScan, int maxPoints)
    {
        lock (_laserCacheLock)
        {
            if (cachedScan == null)
                return null;

            var scan = cachedScan.Value;
            if (scan.Ranges == null || scan.Ranges.Length == 0)
                return null;

            var result = new List<(float angle, float range)>();
            
            // Target: 1 degree = π/180 radians ≈ 0.01745 rad
            const float targetAngleIncrement = (float)(Math.PI / 180.0);
            
            // Calculate how many indices to skip for ~1 degree spacing
            int skipCount = Math.Max(1, (int)Math.Round(targetAngleIncrement / scan.AngleIncrement));
            
            // Sample points at ~1 degree intervals
            for (int i = 0; i < scan.Ranges.Length; i += skipCount)
            {
                float range = (float)scan.Ranges[i];
                
                // Only include valid ranges (within min/max limits)
                if (range >= scan.RangeMin && range <= scan.RangeMax && !float.IsNaN(range) && !float.IsInfinity(range))
                {
                    float angle = (float)(scan.AngleMin + i * scan.AngleIncrement);
                    result.Add((angle, range));
                    
                    // Limit to maxPoints per lidar
                    if (result.Count >= maxPoints)
                        break;
                }
            }
            
            return result;
        }
    }

    /// <summary>
    /// Internal method to return full-resolution laser scan points.
    /// </summary>
    private List<(float angle, float range)>? GetFullLaserScanInternal(LaserScan? cachedScan)
    {
        lock (_laserCacheLock)
        {
            if (cachedScan == null)
                return null;

            var scan = cachedScan.Value;
            if (scan.Ranges == null || scan.Ranges.Length == 0)
                return null;

            var result = new List<(float angle, float range)>(scan.Ranges.Length);

            for (int i = 0; i < scan.Ranges.Length; i++)
            {
                float range = (float)scan.Ranges[i];

                // Keep all finite points from iLidar data (no downsampling).
                if (!float.IsNaN(range) && !float.IsInfinity(range))
                {
                    float angle = (float)(scan.AngleMin + i * scan.AngleIncrement);
                    result.Add((angle, range));
                }
            }

            return result;
        }
    }

    /// <summary>
    /// Log XLOC pose and diagnostics to console (called by timer)
    /// </summary>
    private void LogPoseAndDiagnostics(object? state)
    {
        XlocClient? client = null;

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

        if (client == null)
            return;

        try
        {
            // Get and log current pose
            var pose = client.GetCurrentPose();
            if (pose.HasValue)
            {
                double yaw = Math.Atan2(
                    2 * (pose.Value.qw * pose.Value.qz + pose.Value.qx * pose.Value.qy),
                    1 - 2 * (pose.Value.qy * pose.Value.qy + pose.Value.qz * pose.Value.qz)
                ) * 180.0 / Math.PI;

                // IMPORTANT: Publish map→odom transform based on current localized pose
                // This updates the TF3 buffer with the latest localization result
                // Navigation uses this transform to know where the robot is in the map frame
                // client.PublishMapToOdomTransform();
            }
            else
            {
                _logger.LogWarning("XLOC POSE: No data available from xloc_get_current_pose()");
            }

            // Get and log diagnostics

            // Log sensor dispatch frequency every ~2 seconds
            var now = DateTime.UtcNow;
            var elapsed = (now - _lastFreqSnapshotTime).TotalSeconds;
            if (elapsed >= 2.0)
            {
                var odomHz = (Interlocked.Read(ref _odometryCount) - _lastOdomCountSnapshot) / elapsed;
                var imuHz = (Interlocked.Read(ref _imuCount) - _lastImuCountSnapshot) / elapsed;
                var l1Hz = (Interlocked.Read(ref _lidar1DispatchCount) - _lastLidar1CountSnapshot) / elapsed;
                var l2Hz = (Interlocked.Read(ref _lidar2DispatchCount) - _lastLidar2CountSnapshot) / elapsed;
                var l3Hz = (Interlocked.Read(ref _lidar3DispatchCount) - _lastLidar3CountSnapshot) / elapsed;

                _logger.LogInformation(
                    "[XLOC-FREQ] Odom: {O:F1}Hz, IMU: {I:F1}Hz, L1: {L1:F1}Hz, L2: {L2:F1}Hz, L3: {L3:F1}Hz",
                    odomHz, imuHz, l1Hz, l2Hz, l3Hz);

                _lastOdomCountSnapshot = Interlocked.Read(ref _odometryCount);
                _lastImuCountSnapshot = Interlocked.Read(ref _imuCount);
                _lastLidar1CountSnapshot = Interlocked.Read(ref _lidar1DispatchCount);
                _lastLidar2CountSnapshot = Interlocked.Read(ref _lidar2DispatchCount);
                _lastLidar3CountSnapshot = Interlocked.Read(ref _lidar3DispatchCount);
                _lastFreqSnapshotTime = now;
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error logging XLOC data to console");
        }
    }

    #region Manual Control Methods

    /// <summary>
    /// Manually activate a map for localization
    /// </summary>
    public bool ActivateMap(string mapFilePath)
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot activate map: XlocIntegrationService not initialized");
                return false;
            }

            return _xlocClient.ActivateMap(mapFilePath);
        }
    }

    /// <summary>
    /// Set initial pose: Tell XLOC where the ROBOT is located in the FIXED map frame
    /// 
    /// IMPORTANT:
    /// - This sets the ROBOT's position (x, y, z) and orientation (roll, pitch, yaw) IN the map frame
    /// - The MAP FRAME itself NEVER moves (always fixed at origin 0,0,0)
    /// - Only the robot's estimated position in the map frame changes
    /// 
    /// Parameters: (x, y, z, roll, pitch, yaw) all in map frame coordinates
    /// - Position (x, y, z): meters in map frame
    /// - Orientation (roll, pitch, yaw): radians (converted to quaternion internally)
    /// </summary>
    public bool SetInitialPose(double x, double y, double z, double roll, double pitch, double yaw)
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot set initial pose: XlocIntegrationService not initialized");
                return false;
            }

            // Guard: skip SetInitialPose if XLOC is in MAPPING (0) or PROCESSING (2) state
            var diagnostics = _xlocClient.GetDiagnostics();
            if (diagnostics != null && diagnostics.XlocState is 0 or 2)
            {
                _logger.LogWarning("Skip SetInitialPose because XLOC state is {State} (0=MAPPING, 2=PROCESSING)", diagnostics.XlocState);
                return false;
            }

            // Get current pose BEFORE setting initial pose
            var poseBefore = _xlocClient.GetCurrentPose();
            _logger.LogWarning("=== XLOC SetInitialPose - BEFORE ===");
            if (poseBefore.HasValue)
            {
                var (px, py, pz, pqx, pqy, pqz, pqw) = poseBefore.Value;
                // Convert quaternion to yaw for logging
                double yawBefore = Math.Atan2(2.0 * (pqw * pqz + pqx * pqy), 1.0 - 2.0 * (pqy * pqy + pqz * pqz));
            }
            else
            {
                _logger.LogWarning("Current pose: NULL (no pose available)");
            }

            // Convert Euler angles (roll, pitch, yaw) to quaternion using full formula
            var cy = Math.Cos(yaw * 0.5);
            var sy = Math.Sin(yaw * 0.5);
            var cp = Math.Cos(pitch * 0.5);
            var sp = Math.Sin(pitch * 0.5);
            var cr = Math.Cos(roll * 0.5);
            var sr = Math.Sin(roll * 0.5);

            var qw = cr * cp * cy + sr * sp * sy;
            var qx = sr * cp * cy - cr * sp * sy;
            var qy = cr * sp * cy + sr * cp * sy;
            var qz = cr * cp * sy - sr * sp * cy;

            var result = _xlocClient.SetInitialPose(x, y, z, qx, qy, qz, qw);
            
            if (result)
            {
                // Get current pose AFTER setting initial pose
                System.Threading.Thread.Sleep(50); // Small delay to let XLOC process
                var poseAfter = _xlocClient.GetCurrentPose();
                if (poseAfter.HasValue)
                {
                    var (px, py, pz, pqx, pqy, pqz, pqw) = poseAfter.Value;
                    double yawAfter = Math.Atan2(2.0 * (pqw * pqz + pqx * pqy), 1.0 - 2.0 * (pqy * pqy + pqz * pqz));
                    var dx = Math.Abs(px - x);
                    var dy = Math.Abs(py - y);
                    var dz = Math.Abs(pz - z);
                    var dyaw = Math.Abs(yawAfter - yaw);
                }
            }
            return result;
        }
    }

    /// <summary>
    /// Change (re-anchor) the map coordinate system origin.
    /// Takes an (x, y, z) offset in meters and Euler angles (roll, pitch, yaw) in radians,
    /// converts them to a pose, and invokes the native XLOC change_map_origin.
    /// </summary>
    public bool ChangeMapOrigin(double x, double y, double z, double roll, double pitch, double yaw)
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot change map origin: XlocIntegrationService not initialized");
                return false;
            }

            // Block when XLOC is MAPPING (0) or PROCESSING (2) to avoid inconsistent map state
            var diagnostics = _xlocClient.GetDiagnostics();
            if (diagnostics != null && diagnostics.XlocState is 0 or 2)
            {
                _logger.LogWarning("Skip ChangeMapOrigin because XLOC state is {State} (0=MAPPING, 2=PROCESSING)", diagnostics.XlocState);
                return false;
            }

            var cy = Math.Cos(yaw * 0.5);
            var sy = Math.Sin(yaw * 0.5);
            var cp = Math.Cos(pitch * 0.5);
            var sp = Math.Sin(pitch * 0.5);
            var cr = Math.Cos(roll * 0.5);
            var sr = Math.Sin(roll * 0.5);

            var qw = cr * cp * cy + sr * sp * sy;
            var qx = sr * cp * cy - cr * sp * sy;
            var qy = cr * sp * cy + sr * cp * sy;
            var qz = cr * cp * sy - sr * sp * cy;

            _logger.LogInformation("ChangeMapOrigin -> pos=({X:F3}, {Y:F3}, {Z:F3}) yaw={Yaw:F3} rad",
                x, y, z, yaw);

            var result = _xlocClient.ChangeMapOrigin(x, y, z, qx, qy, qz, qw);
            if (result)
            {
                _logger.LogInformation("Map origin changed successfully");

                // Persist the new origin to the map's YAML on disk so that subsequent calls to
                // /api/xloc/gridmap/static (which override the native origin with the YAML value)
                // return the same origin that native XLOC now uses. Without this, the map bytes
                // and the origin served to the client are desynchronized and the map renders
                // in the wrong place until the user reloads the map.
                try
                {
                    var nativeGrid = _xlocClient.GetStaticGridMap(reloadFromFile: false);
                    var activeMapName = diagnostics?.CurrentActiveMap;
                    if (nativeGrid != null && !string.IsNullOrWhiteSpace(activeMapName))
                    {
                        var newYaw = QuaternionToYaw(
                            nativeGrid.Origin.Qx,
                            nativeGrid.Origin.Qy,
                            nativeGrid.Origin.Qz,
                            nativeGrid.Origin.Qw);

                        UpdateMapYamlOrigin(activeMapName!, nativeGrid.Origin.X, nativeGrid.Origin.Y, newYaw);
                    }
                    else
                    {
                        _logger.LogWarning("ChangeMapOrigin: cannot persist YAML origin (native grid or active map name missing)");
                    }
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "ChangeMapOrigin: failed to persist new origin to YAML");
                }
            }
            else
            {
                _logger.LogWarning("Failed to change map origin");
            }
            return result;
        }
    }

    private static double QuaternionToYaw(double qx, double qy, double qz, double qw)
    {
        // ZYX yaw: atan2(2*(qw*qz + qx*qy), 1 - 2*(qy^2 + qz^2))
        return Math.Atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
    }

    /// <summary>
    /// Rewrite the `origin: [x, y, yaw]` line of the active map's YAML file so the
    /// static-gridmap endpoint (which reads origin from YAML) returns the same value
    /// that native XLOC now has in memory after xloc_change_map_origin.
    /// </summary>
    private void UpdateMapYamlOrigin(string activeMapName, double x, double y, double yaw)
    {
        var mapsDir = XlocPaths.GetMapsDirectory();
        var folderName = System.IO.Path.GetFileName(activeMapName.TrimEnd(System.IO.Path.DirectorySeparatorChar, '/'));
        if (string.IsNullOrEmpty(folderName))
        {
            _logger.LogWarning("UpdateMapYamlOrigin: active map name '{Name}' did not resolve to a folder", activeMapName);
            return;
        }

        var mapFolder = System.IO.Path.Combine(mapsDir, folderName);
        if (!System.IO.Directory.Exists(mapFolder))
        {
            _logger.LogWarning("UpdateMapYamlOrigin: map folder not found: {Folder}", mapFolder);
            return;
        }

        var yamlFiles = System.IO.Directory.GetFiles(mapFolder, "*.yaml");
        if (yamlFiles.Length == 0)
        {
            _logger.LogWarning("UpdateMapYamlOrigin: no YAML file found in {Folder}", mapFolder);
            return;
        }

        var yamlPath = yamlFiles[0];
        var ci = System.Globalization.CultureInfo.InvariantCulture;
        var newOriginLine = string.Format(ci, "origin: [{0:F6}, {1:F6}, {2:F6}]", x, y, yaw);

        var lines = System.IO.File.ReadAllLines(yamlPath);
        bool replaced = false;
        for (int i = 0; i < lines.Length; i++)
        {
            if (lines[i].TrimStart().StartsWith("origin:", StringComparison.Ordinal))
            {
                lines[i] = newOriginLine;
                replaced = true;
                break;
            }
        }
        if (!replaced)
        {
            // Preserve the rest of the file by appending an origin line if one wasn't present.
            var appended = new string[lines.Length + 1];
            Array.Copy(lines, appended, lines.Length);
            appended[lines.Length] = newOriginLine;
            lines = appended;
        }

        System.IO.File.WriteAllLines(yamlPath, lines);
        _logger.LogInformation("UpdateMapYamlOrigin: {Yaml} -> {Line}", yamlPath, newOriginLine);
    }

    /// <summary>
    /// Manually start localization mode
    /// </summary>
    public bool StartLocalization()
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot start localization: XlocIntegrationService not initialized");
                return false;
            }

            _logger.LogInformation("Attempting to start localization...");
            var result = _xlocClient.StartLocalization();
            if (result)
            {
                _logger.LogInformation("Localization started manually");
            }
            else
            {
                _logger.LogWarning("Failed to start localization - check previous log messages for error details");
            }
            return result;
        }
    }

    /// <summary>
    /// Manually stop localization mode
    /// </summary>
    public bool StopLocalization()
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot stop localization: XlocIntegrationService not initialized");
                return false;
            }

            var result = _xlocClient.StopLocalization();
            if (result)
            {
                LastLocalizationStopUtc = DateTime.UtcNow;
                _logger.LogInformation("Localization stopped manually");
            }
            return result;
        }
    }

    /// <summary>
    /// Manually start mapping mode
    /// </summary>
    public bool StartMapping()
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot start mapping: XlocIntegrationService not initialized");
                return false;
            }

            var result = _xlocClient.StartMapping();
            if (result)
            {
                _logger.LogInformation("Mapping started manually");
            }
            return result;
        }
    }

    /// <summary>
    /// Manually stop mapping and save to file
    /// </summary>
    public bool StopMapping(string saveMapFilePath)
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot stop mapping: XlocIntegrationService not initialized");
                return false;
            }

            var result = _xlocClient.StopMapping(saveMapFilePath);
            if (result)
            {
                _logger.LogInformation("Mapping stopped and saved to: {MapFile}", saveMapFilePath);
            }
            return result;
        }
    }

    /// <summary>
    /// Reset SLAM error state - clears previous trajectory state
    /// </summary>
    public bool ResetSlamError()
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot reset SLAM error: XlocIntegrationService not initialized");
                return false;
            }

            _logger.LogInformation("Resetting SLAM error state...");
            var result = _xlocClient.ResetSlamError();
            if (result)
            {
                _logger.LogInformation("SLAM error reset successfully");
            }
            else
            {
                _logger.LogWarning("Failed to reset SLAM error");
            }
            return result;
        }
    }

    /// <summary>
    /// Start updating existing map (only allowed when in localization mode)
    /// </summary>
    public bool StartUpdateMap()
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot start update map: XlocIntegrationService not initialized");
                return false;
            }

            _logger.LogInformation("Attempting to start map update...");
            var result = _xlocClient.StartUpdateMap();
            if (result)
            {
                _logger.LogInformation("Map update started successfully");
            }
            else
            {
                _logger.LogWarning("Failed to start map update - check that robot is in localization mode");
            }
            return result;
        }
    }

    /// <summary>
    /// Stop updating map and optionally save the updated map
    /// </summary>
    public bool StopUpdateMap(bool saveUpdatedMap = true)
    {
        lock (_lock)
        {
            if (!_isInitialized || _xlocClient == null)
            {
                _logger.LogWarning("Cannot stop update map: XlocIntegrationService not initialized");
                return false;
            }

            _logger.LogInformation("Stopping map update (save={Save})...", saveUpdatedMap);
            var result = _xlocClient.StopUpdateMap(saveUpdatedMap);
            if (result)
            {
                _logger.LogInformation("Map update stopped successfully (save={Save})", saveUpdatedMap);
            }
            else
            {
                _logger.LogWarning("Failed to stop map update");
            }
            return result;
        }
    }

    #endregion

    /// <summary>
    /// Broadcast current pose to all SignalR clients
    /// </summary>
    private async Task BroadcastPoseToClientsAsync()
    {
        if (_poseHubContext == null)
            return;

        try
        {
            var pose = GetCurrentPose();
            if (!pose.HasValue)
                return;

            var (x, y, z, qx, qy, qz, qw) = pose.Value;

            // Convert quaternion to yaw
            double yaw = Math.Atan2(2.0 * (qw * qz + qx * qy),
                                    1.0 - 2.0 * (qy * qy + qz * qz));

            var poseData = new XlocPoseData
            {
                X = x,
                Y = y,
                Z = z,
                Yaw = yaw,
                QuaternionX = qx,
                QuaternionY = qy,
                QuaternionZ = qz,
                QuaternionW = qw,
                Timestamp = DateTime.UtcNow,
                IsActive = _isInitialized,
                XlocState = 0,  // Not available without diagnostics call
                MapName = string.Empty,
                Reliability = 0.0,
                MatchingScore = 0.0
            };

            await _poseHubContext.Clients.All.SendAsync("ReceivePose", poseData);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error broadcasting pose to SignalR clients");
        }
    }

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

        _disposed = true;

        try
        {
            // Stop event handlers first
            lock (_lock)
            {
                _isInitialized = false;
            }

            UnregisterLidarEventDispatch();
            Thread.Sleep(500);
            UnregisterOdomImuEventDispatch();

            _imuTimer?.Dispose();
            _odomTimer?.Dispose();
            _lidarTimer?.Dispose();
            _loggingTimer?.Dispose();
            _asyncDispatcher?.Dispose();
            _xlocClient?.Dispose();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing XlocIntegrationService");
        }
    }
}

/// <summary>
/// Xloc operation mode
/// </summary>
public enum XlocMode
{
    None,
    Localization,
    Mapping
}

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

    /// <summary>
    /// Operation mode: Localization or Mapping
    /// </summary>
    public XlocMode Mode { get; set; } = XlocMode.Localization;

    /// <summary>
    /// IMU data dispatch interval in milliseconds (default: 1ms = 1000Hz)
    /// </summary>
    public int ImuDispatchIntervalMs { get; set; } = 1;

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

    /// <summary>
    /// Lidar data dispatch interval in milliseconds (default: 1ms = 1000Hz)
    /// </summary>
    public int LidarDispatchIntervalMs { get; set; } = 1;

    /// <summary>
    /// Update rate in Hz (DEPRECATED - use individual sensor intervals instead)
    /// </summary>
    [Obsolete("Use ImuDispatchIntervalMs, OdomDispatchIntervalMs, LidarDispatchIntervalMs instead")]
    public int UpdateRateHz { get; set; } = 20;

    /// <summary>
    /// Path to map file for localization mode
    /// </summary>
    public string MapFilePath { get; set; } = string.Empty;

    /// <summary>
    /// Path to save map file when stopping mapping mode
    /// </summary>
    public string SaveMapFilePath { get; set; } = string.Empty;

    /// <summary>
    /// Enable dispatching EKF filtered odometry
    /// </summary>
    public bool EnableEKFOdometry { get; set; } = true;

    /// <summary>
    /// Sensor ID for EKF odometry
    /// </summary>
    public string EKFOdometrySensorId { get; set; } = "ekf_odometry";

    /// <summary>
    /// Enable dispatching raw wheel odometry (usually not needed if EKF is enabled)
    /// </summary>
    public bool EnableRawOdometry { get; set; } = false;

    /// <summary>
    /// Sensor ID for raw odometry
    /// </summary>
    public string RawOdometrySensorId { get; set; } = "wheel_odometry";

    /// <summary>
    /// Enable dispatching IMU data
    /// </summary>
    public bool EnableIMU { get; set; } = true;

    /// <summary>
    /// IMU device ID
    /// </summary>
    public string ImuDeviceId { get; set; } = string.Empty;

    /// <summary>
    /// Sensor ID for IMU
    /// </summary>
    public string ImuSensorId { get; set; } = "imu0";

    /// <summary>
    /// IMU frame ID
    /// </summary>
    public string ImuFrameId { get; set; } = "imu_link";

    /// <summary>
    /// Enable dispatching LIDAR data
    /// </summary>
    public bool EnableLidar1 { get; set; } = true;

    public bool EnableLidar2 { get; set; } = true;

    public bool EnableLidar3 { get; set; } = true;

    /// <summary>
    /// LIDAR device ID
    /// </summary>
    public string Lidar1DeviceId { get; set; } = "scan_1";

    /// <summary>
    /// Sensor ID for LIDAR 1 (rear center)
    /// </summary>
    public string Lidar1SensorId { get; set; } = "scan_1";

    /// <summary>
    /// LIDAR 2 device ID (right side)
    /// </summary>
    public string Lidar2DeviceId { get; set; } = "scan_2";

    /// <summary>
    /// Sensor ID for LIDAR 2 (right side)
    /// </summary>
    public string Lidar2SensorId { get; set; } = "scan_2";

    /// <summary>
    /// LIDAR 3 device ID (left side)
    /// </summary>
    public string Lidar3DeviceId { get; set; } = "scan_3";

    /// <summary>
    /// Sensor ID for LIDAR 3 (left side)
    /// </summary>
    public string Lidar3SensorId { get; set; } = "scan_3";

    /// <summary>
    /// Interval in seconds to log statistics
    /// </summary>
    public int StatsLogIntervalSeconds { get; set; } = 10;

    /// <summary>
    /// When true, <see cref="XlocAutoLocalizationHostedService"/> polls diagnostics and calls
    /// StartLocalization when XLOC enters READY (3) with an active map.
    /// </summary>
    public bool AutoStartLocalizationOnReady { get; set; }

    /// <summary>
    /// How often to poll diagnostics for auto-start (milliseconds).
    /// </summary>
    public int AutoStartLocalizationPollIntervalMs { get; set; } = 500;

    /// <summary>
    /// After a successful stop, do not auto-start localization for this many milliseconds.
    /// </summary>
    public int AutoStartLocalizationCooldownAfterStopMs { get; set; } = 10_000;
}