using CartographerSharp.Mapping;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Sensor;
using Microsoft.AspNetCore.SignalR;
using Microsoft.Extensions.Options;
using RobotNet10.RobotApp.Shared;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

public partial class CartographerService
{
    #region Idle State

    private void OnIdleStateEntry()
    {
        lock (_lock)
        {
            _currentState = SLAMState.Idle;
            _lastError = null;
            _lastException = null;
        }
        _ = NotifyStateChangedAsync(SLAMState.Idle);
    }

    #endregion

    #region Initializing State

    private async void OnInitializingStateEntry()
    {
        lock (_lock)
        {
            _currentState = SLAMState.Initializing;
            _lastError = null;
        }
        _ = NotifyStateChangedAsync(SLAMState.Initializing);

        try
        {
            if (_config.Enable)
            {
                // Full SLAM mode: initialize devices and sensor pipeline
                // Wait for devices to be loaded and connected
                var devicesLoaded = await _deviceProvider.WaitForDevicesLoadedAsync(TimeSpan.FromSeconds(30));
                if (!devicesLoaded)
                {
                    _logger.LogError("CartographerService: Timeout waiting for devices to be loaded");
                    throw new TimeoutException("Timeout waiting for devices to be loaded");
                }

                var devicesConnected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(60));
                if (!devicesConnected)
                {
                    _logger.LogWarning("Some devices are not connected, but continuing initialization");
                }

                // Initialize sensor manager (subscribes to all sensors)
                if (_sensorPipeline != null)
                {
                    await _sensorPipeline.InitializeAsync();
                }

                // Create MapBuilder and add trajectory builder using helper
                _mapBuilder = MapBuilderHelper.CreateMapBuilder(_config);
                (_trajectoryId, _trajectoryBuilder) = _mapBuilder.AddTrajectoryBuilder(_config);

                // Check for auto-resume: if CurrentMap.name exists with valid map and pose, auto-start localization
                var savedMapName = LoadCurrentMapName();
                if (!string.IsNullOrEmpty(savedMapName) && MapExists(savedMapName) && PoseFileExists(savedMapName))
                {
                    var savedPose = LoadPoseFromFile(savedMapName);
                    if (savedPose.HasValue)
                    {
                        _logger.LogInformation(
                            "CartographerService: Auto-resume detected - Map: {MapName}, Pose: ({X:F3}, {Y:F3}, {Z:F3})",
                            savedMapName, savedPose.Value.Position.X, savedPose.Value.Position.Y, savedPose.Value.Position.Z);

                        // Set pending localization data
                        lock (_localizationLock)
                        {
                            _pendingLocalizationMapName = savedMapName;
                            _pendingLocalizationInitialPose = savedPose;
                            _isSetInitialPoseFlow = false;
                        }

                        // Fire InitializationComplete to transition to Ready, then StartLocalization to auto-resume
                        FireStateMachine(CartographerTrigger.InitializationComplete);
                        FireStateMachine(CartographerTrigger.StartLocalization);
                        return;
                    }
                }
            }
            FireStateMachine(CartographerTrigger.InitializationComplete);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Initialization failed");
            lock (_lock)
            {
                _lastError = ex.Message;
                _lastException = ex;
            }
            FireStateMachine(CartographerTrigger.InitializationFailed);
        }
    }

    #endregion

    #region Ready State

    private void OnReadyStateEntry()
    {
        lock (_lock)
        {
            _currentState = SLAMState.Ready;
        }

        // Pause sensor subscriptions to stop receiving new data
        _sensorPipeline?.PauseSubscriptions();

        // Wait for in-flight sensor data to be processed
        _sensorPipeline?.WaitForProcessingComplete(200);

        // Notify state change (fire-and-forget)
        _ = NotifyStateChangedAsync(SLAMState.Ready);
    }

    #endregion

    #region Relocalizing State

    private void OnInitializingLocalizingStateEntry()
    {
        lock (_lock)
        {
            _currentState = SLAMState.Relocalizing;
        }

        // Notify state change
        _ = NotifyStateChangedAsync(SLAMState.Relocalizing);

        if (_config.Enable)
        {
            // Start background task to handle localization setup
            _ = Task.Run(async () =>
            {
                try
                {
                    // Get pending data and determine flow type
                    string? mapName;
                    Pose initialPose;
                    bool isSetInitialPoseFlow;

                    lock (_localizationLock)
                    {
                        mapName = _pendingLocalizationMapName;
                        initialPose = _pendingLocalizationInitialPose ?? new();
                        isSetInitialPoseFlow = _isSetInitialPoseFlow;
                        _isSetInitialPoseFlow = false; // Reset flag after reading
                    }

                    if (string.IsNullOrEmpty(mapName))
                    {
                        throw new InvalidOperationException("No map name set for localization");
                    }

                    _logger.LogInformation("CartographerService: Background localization task started for map: {MapName}, isSetInitialPoseFlow: {IsSetInitialPoseFlow}", mapName, isSetInitialPoseFlow);

                    IMapBuilder? mapBuilder;
                    bool useMcl = _config.Mcl.Enabled && _mcl != null;

                    // =================================================================================
                    // STEP 1: Prepare MapBuilder
                    // =================================================================================
                    if (isSetInitialPoseFlow)
                    {
                        // SetInitialPose flow: Finish old trajectory, reuse existing MapBuilder
                        int oldTrajectoryId;
                        lock (_lock)
                        {
                            mapBuilder = _mapBuilder ?? throw new InvalidOperationException("MapBuilder is null during SetInitialPose flow");

                            oldTrajectoryId = _trajectoryId;

                            // Finish old trajectory if exists
                            if (oldTrajectoryId >= 0)
                            {
                                _logger.LogInformation("CartographerService: Finishing old trajectory {TrajectoryId} before restarting with new pose", oldTrajectoryId);
                                mapBuilder.FinishTrajectory(oldTrajectoryId);
                                _trajectoryBuilder = null;
                                _trajectoryId = -1;
                            }
                        }

                        // Wait for trajectory to finish
                        if (oldTrajectoryId >= 0)
                        {
                            await mapBuilder.WaitForTrajectoryFinishedAsync(oldTrajectoryId, CancellationToken.None);

                            // Verify trajectory is finished (defensive check)
                            int currentTrajId;
                            lock (_lock)
                            {
                                currentTrajId = _trajectoryId;
                            }

                            if (currentTrajId != -1)
                            {
                                _logger.LogError("CartographerService: Trajectory {OldId} not finished after wait, current trajectory: {CurrentId}",
                                    oldTrajectoryId, currentTrajId);
                                throw new InvalidOperationException($"Old trajectory {oldTrajectoryId} not finished properly");
                            }

                            _logger.LogInformation("CartographerService: Verified trajectory {TrajectoryId} is finished", oldTrajectoryId);
                        }
                    }
                    else
                    {
                        // StartLocalization flow: Load map from storage
                        _logger.LogInformation("CartographerService: Loading map from storage for map: {MapName}", mapName);

                        // Load map files directly from configuration
                        var loadResult = LoadMapFromDirectory(mapName)
                                            ?? throw new InvalidOperationException($"CartographerService: Failed to load map files: {mapName}");

                        mapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
                            loadResult,
                            _config,
                            _logger,
                            loadFrozenState: true);

                        lock (_lock)
                        {
                            // Reset trajectory from old MapBuilder (created in Initializing state)
                            // before assigning new MapBuilder from pbstream
                            _trajectoryId = -1;
                            _trajectoryBuilder = null;

                            _mapBuilder = mapBuilder;
                            _currentMapName = mapName;
                        }

                        // Load occupancy grid from PGM
                        _occupancyGridManager.LoadFromPgm(mapName);
                    }

                    // =================================================================================
                    // STEP 2: Determine if we can use MCL
                    // =================================================================================
                    bool canUseMcl = useMcl && _occupancyGridManager.OccupancyGridMcl != null;
                    _logger.LogInformation("CartographerService: MCL check - useMcl: {UseMcl}, OccupancyGridMcl != null: {GridNotNull}, canUseMcl: {CanUseMcl}",
                        useMcl, _occupancyGridManager.OccupancyGridMcl != null, canUseMcl);

                    // =================================================================================
                    // STEP 3: Resume sensors AFTER map is prepared (only if sensor pipeline exists)
                    // =================================================================================
                    _sensorPipeline?.ResumeSubscriptions();

                    // =================================================================================
                    // STEP 4: Start localization - MCL path or non-MCL path
                    // =================================================================================
                    if (canUseMcl && _mcl != null && _mclProcessor != null)
                    {
                        if (_occupancyGridManager.OccupancyGridMcl is null)
                        {
                            throw new InvalidOperationException("OccupancyGridMcl is null despite canUseMcl being true");
                        }
                        // MCL path: Start MCL processor, trajectory will be added when MCL converges
                        var grid = _occupancyGridManager.OccupancyGridMcl;
                        var seedPose = initialPose;

                        _mcl.SetMap(grid);
                        _mcl.SetInitialPose(seedPose, useTightNoise: true);

                        var effectivePrimary = GetEffectiveMclPrimaryLidarIdAndLog();
                        SetMclBaseLinkToLaser(effectivePrimary, _mcl);

                        // Start MCL processor (will run until convergence, then fire MclConverged trigger in RunMclOnScan)
                        _mclProcessor.Start(seedPose, effectivePrimary, grid);

                        _logger.LogInformation(
                            "CartographerService: MCL started for map {MapName} with pose [{X}, {Y}, {Z}]; trajectory will be added when MCL converges.",
                            mapName, seedPose.Position.X, seedPose.Position.Y, seedPose.Position.Z);

                        OnPoseUpdated(seedPose, null);

                        // Note: MclConverged trigger will be fired by RunMclOnScan when MCL converges
                    }
                    else
                    {
                        // Non-MCL path: Add trajectory immediately, then fire MclSkipped
                        lock (_lock)
                        {
                            if (_mapBuilder != null)
                            {
                                _logger.LogInformation("CartographerService: Adding trajectory without MCL (MCL disabled or no occupancy grid) with initial pose: [{X}, {Y}, {Z}]",
                                    initialPose.Position.X, initialPose.Position.Y, initialPose.Position.Z);
                                var (trajId, trajBuilder, poseInMapFrame) = _mapBuilder.AddLocalizationTrajectoryBuilder(_config, initialPose);
                                _trajectoryId = trajId;
                                _trajectoryBuilder = trajBuilder;
                                // NOTE: Don't call StartRelocalization - AddLocalizationTrajectoryBuilder already
                                // sets InitialTrajectoryPose with the correct pose. StartRelocalization would
                                // trigger redundant constraint-based relocalization and cause excessive optimization.

                                OnPoseUpdated(initialPose, null);
                            }
                        }

                        // Fire MclSkipped to transition to Localizing
                        _logger.LogInformation("CartographerService: Trajectory added without MCL; firing MclSkipped to transition to Localizing");
                        FireStateMachine(CartographerTrigger.MclSkipped);
                    }

                    _logger.LogInformation("CartographerService: Background localization task completed successfully for map: {MapName}", mapName);
                }
                catch (Exception ex)
                {
                    _logger.LogError(ex, "CartographerService: Background localization task failed");

                    // Update error tracking
                    lock (_lock)
                    {
                        _lastError = ex.Message;
                        _lastException = ex;
                        _mapBuilder = null;
                        _trajectoryBuilder = null;
                        _trajectoryId = -1;
                        _currentMapName = null;
                    }

                    // Fire ErrorOccurred trigger to transition to Error state
                    FireStateMachine(CartographerTrigger.ErrorOccurred);
                }
            });
        }
        else
        {
            if (string.IsNullOrEmpty(_pendingLocalizationMapName))
            {
                FireStateMachine(CartographerTrigger.ErrorOccurred);
                _logger.LogError("Start localization with no map name");
            }
            else
            {
                _currentMapName = _pendingLocalizationMapName;
                if (_pendingLocalizationInitialPose.HasValue)
                {
                    var initPose = _pendingLocalizationInitialPose ?? new();
                    OnPoseUpdated(initPose, null);
                }
                // Load occupancy grid from PGM
                _occupancyGridManager.LoadFromPgm(_pendingLocalizationMapName);
                FireStateMachine(CartographerTrigger.MclSkipped);
            }
        }
    }

    private void OnInitializingLocalizingStateExit()
    {
        // Pause sensors when leaving this state (will be resumed again in Localizing if transitioning there)
        _sensorPipeline?.PauseSubscriptions();
        _sensorPipeline?.WaitForProcessingComplete(200);
    }

    #endregion

    #region Localizing State

    private void OnLocalizingStateEntry()
    {
        lock (_lock)
        {
            _currentState = SLAMState.Localizing;
        }

        // Log trajectory builder status for debugging
        _logger.LogInformation(
            "CartographerService: Entered Localizing state - trajectoryBuilder={TbNotNull}, trajectoryId={TrajId}",
            _trajectoryBuilder != null, _trajectoryId);

        try
        {
            // Save current map name to file for auto-resume on next startup
            var mapName = _currentMapName;
            if (!string.IsNullOrEmpty(mapName))
            {
                SaveCurrentMapName(mapName);
            }

            // Reset drift detector for new localization session
            _driftDetector.Reset();

            // Start pose sync stopwatch for periodic pose saving
            _poseSyncStopwatch.Restart();

            // Start point cloud update stopwatch for throttled point cloud updates
            _pointCloudUpdateStopwatch.Restart();

            // Just resume subscriptions - no special handling needed
            // (SetInitialPose flow is handled in OnInitializingLocalizingStateEntry)
            _logger.LogInformation("CartographerService: Resuming sensor subscriptions for Localizing state");
            _sensorPipeline?.ResumeSubscriptions();
            _logger.LogInformation("CartographerService: Sensor subscriptions resumed successfully");

            // Start background pose extrapolation thread at 100Hz
            StartPoseThread(10);
            _logger.LogInformation("CartographerService: Pose thread started for Localizing state");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Error during OnLocalizingStateEntry");
            throw;
        }

        _ = NotifyStateChangedAsync(SLAMState.Localizing);
    }

    private void OnLocalizingStateExit()
    {
        // Pause sensor subscriptions when leaving Localizing state
        _sensorPipeline?.PauseSubscriptions();
        _sensorPipeline?.WaitForProcessingComplete(200);

        // Stop background pose thread before cleanup
        StopPoseThread();

        // Stop MCL processor if running
        _mclProcessor?.Stop();

        // Stop pose sync stopwatch and save final pose
        _poseSyncStopwatch.Stop();
        var mapName = _currentMapName;
        if (!string.IsNullOrEmpty(mapName))
        {
            SavePoseToFile(mapName, Volatile.Read(ref _poseSnapshot).Pose);
        }

        // Check if this is SetInitialPose flow (transition back to InitializingLocalizing)
        bool isSetInitialPoseFlow;
        lock (_localizationLock)
        {
            isSetInitialPoseFlow = _isSetInitialPoseFlow;
        }

        // Cleanup localization resources when transitioning to Ready (StopLocalization trigger)
        // But keep resources when transitioning back to InitializingLocalizing (SetInitialPose) or self-transitioning
        var nextState = State; // Will be updated after this exits
        if (!isSetInitialPoseFlow && (nextState == SLAMState.Ready || _currentState == SLAMState.Ready))
        {
            lock (_lock)
            {
                _mapBuilder?.FinishTrajectoryAndDispose(_trajectoryId);
                _trajectoryBuilder = null;
                _trajectoryId = -1;
                _mapBuilder = null;
                _currentMapName = null;

                _poseCovariance = null;

                _constraintCache.Invalidate();
            }

            Volatile.Write(ref _poseSnapshot, PoseSnapshot.Empty);

            // Clear localization data from processors
            _slamResultProcessor?.ClearLocalizationData();

            _logger.LogInformation("CartographerService: Localization resources cleaned up");
        }
        else if (isSetInitialPoseFlow)
        {
            _logger.LogInformation("CartographerService: SetInitialPose flow detected, keeping MapBuilder for restart");
        }
    }

    #endregion

    #region ScanMapping State

    private void OnScanMappingStateEntry()
    {
        if (!_config.Enable) return;
        lock (_lock)
        {
            _currentState = SLAMState.ScanMapping;
        }
        Volatile.Write(ref _poseSnapshot, PoseSnapshot.Empty);
        Volatile.Write(ref _lastMatchingScore, -1.0);
        _ = NotifyStateChangedAsync(SLAMState.ScanMapping);

        _ = Task.Run(async () =>
        {
            // IMPORTANT: Sensors are already PAUSED from previous state (OnScanMappingStateExit or OnReadyStateEntry)
            // This ensures no sensor data is processed before we set the initial pose

            // Wait for in-flight sensor data to be processed
            _sensorPipeline?.WaitForProcessingComplete(200);

            // Reset processors for new scan mapping session
            _slamResultProcessor?.ResetForNewSession();
            _occupancyGridManager.ResetCounter();

            // Reset MapBuilder if there are existing trajectory states (theo xloc flow)
            if (_mapBuilder != null && _mapBuilder.DisposeIfHasTrajectories())
            {
                _trajectoryBuilder = null;
                _trajectoryId = -1;
                _mapBuilder = null;
            }

            // STEP 2: Perform wall alignment to calculate aligned pose
            Pose wallAlignedPose = CalculateWallAlignedPoseAsync();
            lock (_wallAlignedPoseLock)
            {
                _wallAlignedPose = wallAlignedPose; // Store for use when saving map
            }

            // STEP 3: Pause sensors again before creating trajectory
            // This prevents sensor data from being processed before we set initial pose
            _sensorPipeline?.PauseSubscriptions();
            _logger.LogInformation("CartographerService: Sensors paused before creating trajectory");

            // Wait for in-flight sensor data to be processed
            _sensorPipeline?.WaitForProcessingComplete(200);

            // STEP 4: Create MapBuilder and add trajectory builder
            if (_mapBuilder == null || _trajectoryBuilder == null)
            {
                _mapBuilder ??= MapBuilderHelper.CreateMapBuilder(_config);

                // Add trajectory builder (without sensor data, extrapolator not initialized yet)
                (_trajectoryId, _trajectoryBuilder) = _mapBuilder.AddTrajectoryBuilder(_config);

                // Log wall-aligned pose (will be applied when saving map via TransformPbstreamOrigin)
                _logger.LogInformation(
                    "CartographerService: Wall alignment calculated for trajectory {TrajectoryId} - " +
                    "Position: [{X:F3}, {Y:F3}, {Z:F3}], Yaw: {Yaw:F3}rad ({YawDeg:F1}°). Will be applied when saving map.",
                    _trajectoryId,
                    wallAlignedPose.Position.X,
                    wallAlignedPose.Position.Y,
                    wallAlignedPose.Position.Z,
                    WallAlignmentHelper.GetYawFromQuaternion(wallAlignedPose.Orientation),
                    WallAlignmentHelper.GetYawFromQuaternion(wallAlignedPose.Orientation) * 180.0 / Math.PI);
            }

            if (_trajectoryBuilder == null)
            {
                _logger.LogWarning("CartographerService: Trajectory builder is null when entering ScanMapping state");
            }

            // STEP 6: Now resume sensors - extrapolator will be initialized with correct initial pose
            _sensorPipeline?.ResumeSubscriptions();
            _logger.LogInformation("CartographerService: Sensors resumed for scan mapping");

            // Start background pose extrapolation thread at 20Hz
            StartPoseThread(50);

            // Start pose sync stopwatch for periodic pose saving
            _poseSyncStopwatch.Restart();

            // Start point cloud update stopwatch for throttled point cloud updates
            _pointCloudUpdateStopwatch.Restart();
        });

    }

    private void OnScanMappingStateExit()
    {
        if (!_config.Enable) return;
        // Pause sensor subscriptions immediately when leaving ScanMapping state
        _sensorPipeline?.PauseSubscriptions();

        // Stop background pose thread before cleanup
        StopPoseThread();

        // Note: Do NOT manually update _currentState here - state machine will handle state transition
        // The state will be updated when ExecuteOnEntry of next state is called

        // Stop pose sync stopwatch and save final pose
        _poseSyncStopwatch.Stop();
        var mapName = _currentMapName;
        if (!string.IsNullOrEmpty(mapName))
        {
            SavePoseToFile(mapName, Volatile.Read(ref _poseSnapshot).Pose);
        }

        // Wait for in-flight sensor data to be processed
        _sensorPipeline?.WaitForProcessingComplete(200);
    }

    #endregion

    #region SavingMap State

    private void OnSavingMapStateEntry()
    {
        if (!_config.Enable) return;
        lock (_lock)
        {
            _currentState = SLAMState.SavingMap;
        }

        // Pause sensor subscriptions to stop receiving new data
        _sensorPipeline?.PauseSubscriptions();

        // Wait for in-flight sensor data to be processed
        // This prevents "Cannot add node to finished trajectory" errors
        _sensorPipeline?.WaitForProcessingComplete(2000);

        // Notify state change (fire-and-forget)
        _ = NotifyStateChangedAsync(SLAMState.SavingMap);

        // Create CancellationTokenSource for save operation (allows cancellation if needed)
        _savingMapCts?.Cancel();
        _savingMapCts?.Dispose();
        _savingMapCts = new CancellationTokenSource();
        var cancellationToken = _savingMapCts.Token;

        // Start background save task to handle async save workflow
        _ = Task.Run(async () =>
        {
            try
            {
                string? mapName;
                lock (_lock)
                {
                    mapName = _currentMapName;
                }

                if (string.IsNullOrEmpty(mapName))
                {
                    throw new InvalidOperationException("No map name set for save operation");
                }

                var mapBuilder = MapBuilder ?? throw new InvalidOperationException("MapBuilder is not available for save operation");
                var trajectoryId = TrajectoryId;

                // Get wall-aligned pose with proper synchronization
                Pose? wallAlignedPose;
                lock (_wallAlignedPoseLock)
                {
                    wallAlignedPose = _wallAlignedPose;
                }

                _logger.LogInformation("CartographerService: Background save task started for map: {MapName}", mapName);

                // Execute save map logic using MapSaveProcessor
                // This handles: finish trajectory, optimization, pbstream, PGM, PNG, JPG, metadata
                if (_mapSaveProcessor == null)
                {
                    throw new InvalidOperationException("MapSaveProcessor is not available (CartographerConfiguration.Enable = false)");
                }

                // Create progress callback to send updates via SignalR
                async Task ProgressCallback(int workItemsAdded, int workItemsCompleted, int percentComplete)
                {
                    try
                    {
                        await _hubContext.Clients.All.SendAsync("OnMapSaveProgress",
                            workItemsAdded, workItemsCompleted, percentComplete, cancellationToken);
                    }
                    catch (Exception ex)
                    {
                        _logger.LogWarning(ex, "CartographerService: Failed to send map save progress update");
                    }
                }

                var mapPath = await _mapSaveProcessor.SaveMapAsync(mapName, mapBuilder, trajectoryId, ProgressCallback, wallAlignedPose, cancellationToken);

                _logger.LogInformation("CartographerService: Background save task completed successfully for map: {MapName}, path: {MapPath}", mapName, mapPath);

                // Save current map name to file for auto-resume on next startup
                SaveCurrentMapName(mapName);

                // Fire MapSaved trigger to transition to Ready state
                FireStateMachine(CartographerTrigger.MapSaved);
            }
            catch (OperationCanceledException)
            {
                _logger.LogWarning("CartographerService: Background save task was cancelled");

                // Update error tracking
                lock (_lock)
                {
                    _lastError = "Save operation was cancelled";
                    _lastException = null;
                }

                // Fire ErrorOccurred trigger to transition to Error state
                FireStateMachine(CartographerTrigger.ErrorOccurred);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "CartographerService: Background save task failed");

                // Update error tracking
                lock (_lock)
                {
                    _lastError = ex.Message;
                    _lastException = ex;
                }

                // Fire ErrorOccurred trigger to transition to Error state
                FireStateMachine(CartographerTrigger.ErrorOccurred);
            }
        }, cancellationToken);
    }

    #endregion

    #region Error State

    private void OnErrorStateEntry()
    {
        lock (_lock)
        {
            _currentState = SLAMState.Error;
            _logger.LogError(
                "CartographerService: Entered Error state. Error: {Error}. " +
                "Call Reset() to return to Idle state and reinitialize.",
                _lastError);

            // Log exception details if available
            if (_lastException != null)
            {
                _logger.LogError(_lastException,
                    "CartographerService: Exception details for Error state");
            }
        }
        _ = NotifyStateChangedAsync(SLAMState.Error);
    }

    #endregion
}