using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
using RobotNet10.Shared.Geometry;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

/// <summary>
/// Immutable snapshot of robot pose with confidence metrics and drift detection status.
/// Reference type enables lock-free atomic swap via volatile field.
/// </summary>
public sealed class PoseSnapshot
{
    public static readonly PoseSnapshot Empty = new();

    /// <summary>Pose in GLOBAL (map) frame.</summary>
    public Pose Pose { get; }

    /// <summary>Pose covariance (Localizing mode only).</summary>
    public Matrix3x3? Covariance { get; }

    /// <summary>
    /// Confidence score:
    /// - ScanMapping: PoseConfidence from scan matcher
    /// - Localizing: LocalizationScore from covariance/constraints/scanMatchScore
    /// - Relocalizing: MCL reliability
    /// </summary>
    public double? Score { get; }

    /// <summary>
    /// Scan match score (PoseConfidence) from Cartographer.
    /// This is the most direct indicator of how well the current scan matches the map.
    /// Available in both ScanMapping and Localizing states.
    /// </summary>
    public double? ScanMatchScore { get; }

    /// <summary>
    /// Drift detection status (Localizing mode only).
    /// Indicates whether the robot may be experiencing map drift.
    /// </summary>
    public DriftDetector.DriftStatus DriftStatus { get; }

    /// <summary>Timestamp when this snapshot was created.</summary>
    public long TimestampTicks { get; }

    public PoseSnapshot()
    {
        Pose = new Pose();
        DriftStatus = DriftDetector.DriftStatus.Stable;
        TimestampTicks = DateTime.UtcNow.Ticks;
    }

    public PoseSnapshot(Pose pose, Matrix3x3? covariance = null, double? score = null)
    {
        Pose = pose;
        Covariance = covariance;
        Score = score;
        DriftStatus = DriftDetector.DriftStatus.Stable;
        TimestampTicks = DateTime.UtcNow.Ticks;
    }

    /// <summary>
    /// Creates a PoseSnapshot with full drift detection information.
    /// </summary>
    public PoseSnapshot(
        Pose pose,
        Matrix3x3? covariance,
        double? score,
        double? scanMatchScore,
        DriftDetector.DriftStatus driftStatus)
    {
        Pose = pose;
        Covariance = covariance;
        Score = score;
        ScanMatchScore = scanMatchScore;
        DriftStatus = driftStatus;
        TimestampTicks = DateTime.UtcNow.Ticks;
    }
}