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

using CartographerSharp.Mapping;
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;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

public partial class CartographerService
{
    #region ISLAMService -- Localization

    public void StartLocalization(string mapName, Pose? initialPose = null)
    {
        // Validate state
        var currentState = State;
        if (currentState != SLAMState.Ready)
        {
            throw new InvalidOperationException($"Cannot start localization from state: {currentState}");
        }

        // Dispose old MapBuilder if exists
        lock (_lock)
        {
            if (_mapBuilder != null)
            {
                _logger.LogInformation("CartographerService: Disposing existing MapBuilder before starting localization");
                (_mapBuilder as IDisposable)?.Dispose();
                _mapBuilder = null;
                _trajectoryBuilder = null;
                _trajectoryId = -1;
                _currentMapName = null;
            }
        }

        // Store pending data for InitializingLocalizing state
        lock (_localizationLock)
        {
            _pendingLocalizationMapName = mapName;
            _pendingLocalizationInitialPose = initialPose;
        }

        // Fire StartLocalization trigger
        // Actual work will be done in InitializingLocalizing.ExecuteOnEntry
        FireStateMachine(CartographerTrigger.StartLocalization);

        _logger.LogInformation("CartographerService: StartLocalization trigger fired for map: {MapName}", mapName);
    }

    public void SetInitialPose(Pose pose)
    {
        // Validate state and get current map name
        string? currentMapName;
        lock (_lock)
        {
            if (_currentState != SLAMState.Localizing && _currentState != SLAMState.Relocalizing)
            {
                throw new InvalidOperationException(
                    $"SetInitialPose is only allowed during Localizing or InitializingLocalizing. Current state: {_currentState}");
            }
            if (_mapBuilder == null)
            {
                if (_config.Enable)
                {
                    FireStateMachine(CartographerTrigger.ErrorOccurred);
                    throw new InvalidOperationException(
                        "Localization not started. Call StartLocalization first.");
                }
            }
            currentMapName = _currentMapName;
        }

        if (string.IsNullOrEmpty(currentMapName))
        {
            throw new InvalidOperationException("No map name available for SetInitialPose");
        }

        // Store pending data for InitializingLocalizing state
        lock (_localizationLock)
        {
            _pendingLocalizationMapName = currentMapName; // Reuse current map
            _pendingLocalizationInitialPose = pose;
            _isSetInitialPoseFlow = true; // Mark as SetInitialPose flow
        }

        _logger.LogInformation("CartographerService: SetInitialPose called with pose [{X}, {Y}, {Z}], transitioning to InitializingLocalizing",
            pose.Position.X, pose.Position.Y, pose.Position.Z);

        // Fire StartLocalization trigger to transition to InitializingLocalizing
        // The InitializingLocalizing state handler will process the restart with new pose
        FireStateMachine(CartographerTrigger.StartLocalization);
    }

    public void StopLocalization() => FireStateMachine(CartographerTrigger.StopLocalization);

    #endregion

    #region ISLAMService -- ScanMapping

    public void StartScanMapping(string mapName)
    {
        // Validate that SLAM is enabled
        if (!_config.Enable) return;

        lock (_lock)
        {
            _currentMapName = mapName;
        }
        FireStateMachine(CartographerTrigger.StartScanMapping);
    }

    public void SaveScanMap()
    {
        // Validate that SLAM is enabled
        if (!_config.Enable) return;

        FireStateMachine(CartographerTrigger.SaveMap);
    }

    #endregion

    #region ISLAMService -- Query

    public List<(int NodeId, Pose Pose)>? GetTrajectoryNodes()
    {
        var trajectoryNodePoses = _mapBuilder?.PoseGraph.GetTrajectoryNodePoses() ?? null;

        if (trajectoryNodePoses == null || trajectoryNodePoses.IsEmpty)
        {
            return [];
        }

        var result = new List<(int NodeId, Pose Pose)>();
        var trajectoryId = _trajectoryId;

        foreach (var kvp in trajectoryNodePoses)
        {
            if (kvp.Id.TrajectoryId == trajectoryId)
            {
                var globalPose = kvp.Data.GlobalPose;
                var pose = Helpers.PoseConverter.ToPose(globalPose);
                result.Add((kvp.Id.NodeIndex, pose));
            }
        }
        return result;
    }

    public IReadOnlyList<Vector3> GetAggregatedSamplePointCloud()
    {
        lock (_pointCloudLock)
        {
            var count = _completedAccumulatedSamplePointClouds.Count;
            return [.. _completedAccumulatedSamplePointClouds];
        }
    }

    public OccupancyGrid? GetOccupancyGrid() => _occupancyGridManager.OccupancyGrid;

    public OccupancyGrid? GetOccupancyGrid(DateTime since) => _occupancyGridManager.GetGrid(since);

    public DateTime LastUpdatedOccupancyGrid => _occupancyGridManager.LastUpdated;

    #endregion
}