BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/MclProcessor.cs

using CartographerSharp.Sensor;
using RobotNet10.RobotApp.SLAM.Cartographer;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Reason for MCL convergence
/// </summary>
public enum MclConvergenceReason
{
    /// <summary>MCL has not converged yet</summary>
    NotConverged,
    /// <summary>Pose is stable and quality criteria met</summary>
    PoseStable,
    /// <summary>Timeout reached without convergence</summary>
    Timeout,
    /// <summary>Max iterations reached without convergence</summary>
    MaxIterations
}

/// <summary>
/// Processes MCL (Monte Carlo Localization) convergence and pose tracking
/// </summary>
public class MclProcessor
{
    private readonly ILogger<MclProcessor> _logger;
    private readonly CartographerConfiguration _config;
    private readonly Lock _lock = new();

    private bool _running;
    private string? _primaryLidarId;
    private DateTime? _stableSinceUtc;
    private DateTime _runningSinceUtc = DateTime.MinValue;
    private int _iterationCount;
    private Pose? _lastPose;
    private Pose _initialPose; // Initial pose for fallback on timeout
    private OdometryData? _lastOdometryData;
    private double _lastOdometryTimestampSec;

    public MclProcessor(CartographerConfiguration config, ILogger<MclProcessor> logger)
    {
        _config = config ?? throw new ArgumentNullException(nameof(config));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
    }

    public bool IsRunning
    {
        get { lock (_lock) { return _running; } }
    }

    public string? PrimaryLidarId
    {
        get { lock (_lock) { return _primaryLidarId; } }
    }

    /// <summary>
    /// Gets the initial pose used when MCL started (for fallback on timeout)
    /// </summary>
    public Pose InitialPose
    {
        get { lock (_lock) { return _initialPose; } }
    }

    /// <summary>
    /// Start MCL with initial seed pose
    /// </summary>
    public void Start(Pose seedPose, string effectivePrimaryLiderId, OccupancyGrid? grid)
    {
        lock (_lock)
        {
            _running = true;
            _primaryLidarId = effectivePrimaryLiderId;
            _iterationCount = 0;
            _lastPose = seedPose;
            _initialPose = seedPose; // Store initial pose for timeout fallback
            _stableSinceUtc = null;
            _runningSinceUtc = DateTime.UtcNow;
            _lastOdometryData = null;
            _lastOdometryTimestampSec = 0;
        }

        _logger.LogInformation("MclProcessor: MCL started with primary lidar: {LidarId}", effectivePrimaryLiderId);
    }

    /// <summary>
    /// Stop MCL
    /// </summary>
    public void Stop()
    {
        lock (_lock)
        {
            _running = false;
            _primaryLidarId = null;
            _stableSinceUtc = null;
            _iterationCount = 0;
            _lastPose = null;
            _lastOdometryData = null;
            _lastOdometryTimestampSec = 0;
        }
    }

    /// <summary>
    /// Update MCL seed pose during running state
    /// </summary>
    public void UpdateSeedPose(Pose newPose)
    {
        lock (_lock)
        {
            if (!_running)
                return;

            _iterationCount = 0;
            _lastPose = null;
            _stableSinceUtc = null;
            _lastOdometryData = null;
            _lastOdometryTimestampSec = 0;
        }

        _logger.LogInformation("MclProcessor: Seed pose updated");
    }

    /// <summary>
    /// Check MCL convergence based on pose stability and quality criteria
    /// </summary>
    /// <returns>True if converged (for any reason), false otherwise</returns>
    public bool CheckConvergence(Pose currentPose, double reliability, double? mae, out int iterationCount)
    {
        return CheckConvergenceWithReason(currentPose, reliability, mae, out iterationCount) != MclConvergenceReason.NotConverged;
    }

    /// <summary>
    /// Check MCL convergence and return the reason
    /// </summary>
    /// <returns>Convergence reason indicating why MCL stopped or NotConverged if still running</returns>
    public MclConvergenceReason CheckConvergenceWithReason(Pose currentPose, double reliability, double? mae, out int iterationCount)
    {
        lock (_lock)
        {
            iterationCount = ++_iterationCount;

            if (_iterationCount < _config.Mcl.ConvergenceMinIterations)
            {
                _lastPose = currentPose;
                return MclConvergenceReason.NotConverged;
            }

            // Calculate pose change
            double poseDist = double.MaxValue;
            double yawDiff = double.MaxValue;

            if (_lastPose.HasValue)
            {
                double dx = currentPose.Position.X - _lastPose.Value.Position.X;
                double dy = currentPose.Position.Y - _lastPose.Value.Position.Y;
                poseDist = Math.Sqrt(dx * dx + dy * dy);

                double yawCur = GetYawFromPose(currentPose);
                double yawLast = GetYawFromPose(_lastPose.Value);
                double dyaw = yawCur - yawLast;
                while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
                while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
                yawDiff = Math.Abs(dyaw);
            }

            _lastPose = currentPose;

            // Check stability criteria
            bool poseStable = poseDist < _config.Mcl.ConvergencePoseChangeThresholdMeters &&
                            yawDiff < _config.Mcl.ConvergenceYawChangeThresholdRad;

            bool qualityGood = !_config.Mcl.EstimateReliability ||
                            (reliability >= _config.Mcl.ConvergenceReliabilityMin &&
                             (!mae.HasValue || mae.Value <= _config.Mcl.ConvergenceMaeMaxMeters));

            var now = DateTime.UtcNow;
            double runningSec = (now - _runningSinceUtc).TotalSeconds;
            double stableDurationSec = _config.Mcl.ConvergenceStableDurationSeconds;
            double timeoutSec = _config.Mcl.ConvergenceTimeoutSeconds;

            if (qualityGood)
            {
                if (poseStable)
                {
                    if (!_stableSinceUtc.HasValue)
                        _stableSinceUtc = now;
                    else if ((now - _stableSinceUtc.Value).TotalSeconds >= stableDurationSec)
                        return MclConvergenceReason.PoseStable;
                }
                else
                {
                    _stableSinceUtc = null;
                }
            }
            else
            {
                _stableSinceUtc = null;
                if (runningSec >= timeoutSec)
                    return MclConvergenceReason.Timeout;
            }

            if (_iterationCount >= _config.Mcl.ConvergenceMaxIterations)
                return MclConvergenceReason.MaxIterations;

            return MclConvergenceReason.NotConverged;
        }
    }

    /// <summary>
    /// Process odometry data for MCL motion model
    /// </summary>
    public void ProcessOdometry(OdometryData odomData)
    {
        lock (_lock)
        {
            if (!_running || !_lastOdometryData.HasValue)
            {
                _lastOdometryData = odomData;
                _lastOdometryTimestampSec = odomData.Time / 10_000_000.0;
                return;
            }

            double currentTimeSec = odomData.Time / 10_000_000.0;
            double deltaTimeSec = currentTimeSec - _lastOdometryTimestampSec;

            if (deltaTimeSec > 0 && deltaTimeSec < 1.0)
            {
                var lastPose = _lastOdometryData.Value.Pose;
                var currentOdomPose = odomData.Pose;

                double dx = currentOdomPose.Translation.X - lastPose.Translation.X;
                double dy = currentOdomPose.Translation.Y - lastPose.Translation.Y;
                double linearX = dx / deltaTimeSec;
                double linearY = dy / deltaTimeSec;

                double yawLast = ((RobotNet10.Shared.Numbers.Quaternion)lastPose.Rotation).ToYawRadian();
                double yawCurrent = ((RobotNet10.Shared.Numbers.Quaternion)currentOdomPose.Rotation).ToYawRadian();
                double dyaw = yawCurrent - yawLast;
                while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
                while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
                double angularZ = dyaw / deltaTimeSec;

                // Store for use by MCL (would be passed to mcl.OnOdom in actual implementation)
                OnOdometryProcessed?.Invoke(deltaTimeSec, linearX, linearY, angularZ);
            }

            _lastOdometryData = odomData;
            _lastOdometryTimestampSec = currentTimeSec;
        }
    }

    // Event to signal odometry processing
    public event Action<double, double, double, double>? OnOdometryProcessed;

    private static double GetYawFromPose(Pose p)
    {
        var q = p.Orientation;
        return Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z));
    }

    public void Dispose()
    {
        lock (_lock)
        {
            Stop();
        }
    }
}