BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.Lifecycle.cs

using System.Globalization;
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

public partial class CartographerService
{
    #region IHostedService

    public async Task StartAsync(CancellationToken cancellationToken)
    {
        try
        {
            // Start trigger thread first (handles deferred state machine triggers from sensor processing)
            StartTriggerThread();

            // Start state machine
            lock (_stateMachineLock) { _stateMachine.Start(); }

            // Fire Start trigger to begin initialization
            FireStateMachine(CartographerTrigger.Start);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Failed to start");
            throw;
        }
    }

    public async Task StopAsync(CancellationToken cancellationToken)
    {
        try
        {
            // Mark as disposed to prevent new operations
            _disposed = true;

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

            // Stop background threads
            StopPoseThread();
            StopTriggerThread();

            // Give a small delay to ensure any in-flight sensor data is processed
            // (PoseGraph2D work queue will handle processing)
            await Task.Delay(100, CancellationToken.None);
            _sensorPipeline?.Dispose();

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

            // Finish trajectory if active, then dispose MapBuilder (stops background threads before state machine shutdown)
            _mapBuilder?.FinishTrajectoryAndDispose(_trajectoryId);
            _trajectoryId = -1;
            _trajectoryBuilder = null;
            _mapBuilder = null;

            FireStateMachine(CartographerTrigger.Shutdown);

            // Stop state machine
            try
            {
                lock (_stateMachineLock)
                {
                    if (_stateMachine.IsRunning)
                    {
                        _stateMachine.Stop();
                    }
                }
            }
            catch (Exception ex)
            {
                _logger.LogWarning(ex, "CartographerService: Error stopping state machine");
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Error during stop");
            throw;
        }
    }

    #endregion

    #region State Persistence

    /// <summary>
    /// Ensures the localizing state directory exists
    /// </summary>
    private string EnsureLocalizingStateDirectory()
    {
        var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
        if (!Directory.Exists(directory))
        {
            Directory.CreateDirectory(directory);
            _logger.LogInformation("CartographerService: Created localizing state directory: {Directory}", directory);
        }
        return directory;
    }

    /// <summary>
    /// Saves the current map name to CurrentMap.name file
    /// </summary>
    private void SaveCurrentMapName(string mapName)
    {
        try
        {
            var directory = EnsureLocalizingStateDirectory();
            var filePath = Path.Combine(directory, "CurrentMap.name");
            File.WriteAllText(filePath, mapName);
            _logger.LogDebug("CartographerService: Saved current map name '{MapName}' to {FilePath}", mapName, filePath);
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "CartographerService: Failed to save current map name");
        }
    }

    /// <summary>
    /// Loads the current map name from CurrentMap.name file
    /// </summary>
    /// <returns>Map name if file exists and is valid, null otherwise</returns>
    private string? LoadCurrentMapName()
    {
        try
        {
            var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
            var filePath = Path.Combine(directory, "CurrentMap.name");
            if (!File.Exists(filePath))
            {
                return null;
            }

            var mapName = File.ReadAllText(filePath).Trim();
            if (string.IsNullOrWhiteSpace(mapName))
            {
                return null;
            }

            _logger.LogDebug("CartographerService: Loaded current map name '{MapName}' from {FilePath}", mapName, filePath);
            return mapName;
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "CartographerService: Failed to load current map name");
            return null;
        }
    }

    /// <summary>
    /// Saves the pose to {mapName}.pose file in text format: x y z qx qy qz qw
    /// </summary>
    private void SavePoseToFile(string mapName, Pose pose)
    {
        try
        {
            var directory = EnsureLocalizingStateDirectory();
            var sanitizedMapName = MapNameHelper.Sanitize(mapName);
            var filePath = Path.Combine(directory, $"{sanitizedMapName}.pose");

            // Format: x y z qx qy qz qw (space-separated, invariant culture for decimal point)
            var content = string.Format(
                CultureInfo.InvariantCulture,
                "{0} {1} {2} {3} {4} {5} {6}",
                pose.Position.X,
                pose.Position.Y,
                pose.Position.Z,
                pose.Orientation.X,
                pose.Orientation.Y,
                pose.Orientation.Z,
                pose.Orientation.W);

            File.WriteAllText(filePath, content);
            _logger.LogDebug("CartographerService: Saved pose to {FilePath}: {Content}", filePath, content);
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "CartographerService: Failed to save pose for map '{MapName}'", mapName);
        }
    }

    /// <summary>
    /// Loads the pose from {mapName}.pose file
    /// </summary>
    /// <returns>Pose if file exists and is valid, null otherwise</returns>
    private Pose? LoadPoseFromFile(string mapName)
    {
        try
        {
            var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
            var sanitizedMapName = MapNameHelper.Sanitize(mapName);
            var filePath = Path.Combine(directory, $"{sanitizedMapName}.pose");

            if (!File.Exists(filePath))
            {
                return null;
            }

            var content = File.ReadAllText(filePath).Trim();
            var parts = content.Split(' ', StringSplitOptions.RemoveEmptyEntries);

            if (parts.Length != 7)
            {
                _logger.LogWarning("CartographerService: Invalid pose file format in {FilePath}, expected 7 values, got {Count}", filePath, parts.Length);
                return null;
            }

            var pose = new Pose
            {
                Position = new Vector3(
                    double.Parse(parts[0], CultureInfo.InvariantCulture),
                    double.Parse(parts[1], CultureInfo.InvariantCulture),
                    double.Parse(parts[2], CultureInfo.InvariantCulture)),
                Orientation = new Quaternion(
                    double.Parse(parts[3], CultureInfo.InvariantCulture),
                    double.Parse(parts[4], CultureInfo.InvariantCulture),
                    double.Parse(parts[5], CultureInfo.InvariantCulture),
                    double.Parse(parts[6], CultureInfo.InvariantCulture))
            };

            _logger.LogDebug("CartographerService: Loaded pose from {FilePath}: ({X}, {Y}, {Z})",
                filePath, pose.Position.X, pose.Position.Y, pose.Position.Z);
            return pose;
        }
        catch (Exception ex)
        {
            _logger.LogWarning(ex, "CartographerService: Failed to load pose for map '{MapName}'", mapName);
            return null;
        }
    }

    /// <summary>
    /// Checks if a pose file exists for the given map
    /// </summary>
    private bool PoseFileExists(string mapName)
    {
        try
        {
            var directory = Path.GetFullPath(_config.MapStorage.LocalizingStateDirectory);
            var sanitizedMapName = MapNameHelper.Sanitize(mapName);
            var filePath = Path.Combine(directory, $"{sanitizedMapName}.pose");
            return File.Exists(filePath);
        }
        catch
        {
            return false;
        }
    }

    #endregion

    #region IDisposable

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

        try
        {
            _disposed = true;

            // Cancel any ongoing save operation
            _savingMapCts?.Cancel();
            _savingMapCts?.Dispose();
            _savingMapCts = null;

            _sensorPipeline?.PauseSubscriptions();
            _sensorPipeline?.WaitForProcessingComplete(200);

            // Stop background threads
            StopPoseThread();
            StopTriggerThread();

            // Dispose pending trigger event
            _pendingTriggerEvent.Dispose();

            _sensorPipeline?.Dispose();

            // Finish trajectory if active, then dispose MapBuilder (safety net if Dispose is called before StopAsync)
            _mapBuilder?.FinishTrajectoryAndDispose(_trajectoryId);
            _trajectoryBuilder = null;
            _trajectoryId = -1;
            _mapBuilder = null;

            _occupancyGridManager.Dispose();
            _slamResultProcessor?.Dispose();
            _mclProcessor?.Dispose();

            lock (_stateMachineLock) { _stateMachine.Stop(); }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Error during Dispose()");
            // Don't throw in Dispose - log and continue cleanup
        }
        finally
        {
            GC.SuppressFinalize(this);
        }
    }

    #endregion
}