I150/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/SlamResultProcessor.cs

using CartographerSharp.Mapping;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
using RobotNet10.Shared.Geometry;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Processes SLAM results from sensor data
/// Handles both ScanMapping and Localization modes with appropriate pose/covariance updates
/// </summary>
public class SlamResultProcessor(ILogger<SlamResultProcessor> _logger)
{
    private readonly Lock _lock = new();

    private int _totalSubmapsCreated;
    private int _scanMappingTrajectoryNodeCount;
    private Matrix3x3? _poseCovariance;
    private int _constraintCount;
    private double _averageConstraintQuality;

    public int TotalSubmapsCreated
    {
        get { lock (_lock) { return _totalSubmapsCreated; } }
    }

    public int TrajectoryNodeCount
    {
        get { lock (_lock) { return _scanMappingTrajectoryNodeCount; } }
    }

    public Matrix3x3? PoseCovariance
    {
        get { lock (_lock) { return _poseCovariance; } }
    }

    public int ConstraintCount
    {
        get { lock (_lock) { return _constraintCount; } }
    }

    public double AverageConstraintQuality
    {
        get { lock (_lock) { return _averageConstraintQuality; } }
    }

    /// <summary>
    /// Process SLAM result for ScanMapping mode
    /// Tracks submap creation and trajectory nodes
    /// </summary>
    public bool ProcessScanMappingResult(
        ITrajectoryBuilder.MatchingResult result,
        SLAMState currentState,
        out IReadOnlyList<CartographerSharp.Mapping.Submap>? newSubmaps)
    {
        newSubmaps = null;

        if (result.InsertionResult?.InsertionSubmaps == null || result.InsertionResult.Value.InsertionSubmaps.Count == 0)
            return false;

        var insertionSubmaps = result.InsertionResult.Value.InsertionSubmaps;
        bool shouldFireSubmapsUpdated = false;

        lock (_lock)
        {
            _totalSubmapsCreated += insertionSubmaps.Count;

            if (currentState == SLAMState.ScanMapping)
            {
                shouldFireSubmapsUpdated = true;
                newSubmaps = insertionSubmaps;

                // Track nodes
                if (result.InsertionResult.Value.ConstantData != null)
                {
                    _scanMappingTrajectoryNodeCount++;
                }
            }
        }

        if (shouldFireSubmapsUpdated)
        {
            _logger.LogDebug("SlamResultProcessor: {Count} new submaps created (Total: {Total})",
                insertionSubmaps.Count, _totalSubmapsCreated);
        }

        return shouldFireSubmapsUpdated;
    }

    /// <summary>
    /// Process SLAM result for Localization mode
    /// Updates covariance and constraint tracking
    /// </summary>
    public void ProcessLocalizationResult(
        Pose currentPose,
        Matrix3x3? cachedCovariance,
        int cachedConstraintCount,
        double cachedAverageQuality)
    {
        lock (_lock)
        {
            _poseCovariance = cachedCovariance;
            _constraintCount = cachedConstraintCount;
            _averageConstraintQuality = cachedAverageQuality;
        }
    }

    /// <summary>
    /// Update covariance from constraint calculation
    /// </summary>
    public void UpdateCovariance(Matrix3x3? covariance, int constraintCount, double averageQuality)
    {
        lock (_lock)
        {
            _poseCovariance = covariance;
            _constraintCount = constraintCount;
            _averageConstraintQuality = averageQuality;
        }
    }

    /// <summary>
    /// Reset counters for new ScanMapping session
    /// </summary>
    public void ResetForNewSession()
    {
        lock (_lock)
        {
            _totalSubmapsCreated = 0;
            _scanMappingTrajectoryNodeCount = 0;
            _poseCovariance = null;
            _constraintCount = 0;
            _averageConstraintQuality = 0.0;
        }
    }

    /// <summary>
    /// Clear localization data
    /// </summary>
    public void ClearLocalizationData()
    {
        lock (_lock)
        {
            _poseCovariance = null;
            _constraintCount = 0;
            _averageConstraintQuality = 0.0;
        }
    }

    public void Dispose()
    {
        // Nothing to dispose currently
    }
}