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

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Detects map drift during localization by monitoring multiple metrics.
/// Combines scan matching quality, odometry residuals, and optional MCL cross-validation.
/// </summary>
public class DriftDetector
{
    #region Configuration

    /// <summary>Configuration for drift detection thresholds and weights</summary>
    public record DriftDetectorConfig
    {
        /// <summary>Window size for moving average calculations</summary>
        public int WindowSize { get; init; } = 20;

        /// <summary>Minimum scan match score to consider "good" (0.0-1.0)</summary>
        public double MinScanMatchScore { get; init; } = 0.4;

        /// <summary>Maximum allowed odometry residual in meters before flagging drift</summary>
        public double MaxOdometryResidual { get; init; } = 0.5;

        /// <summary>Maximum allowed MCL-Cartographer divergence in meters</summary>
        public double MaxMclDivergence { get; init; } = 0.3;

        /// <summary>Maximum allowed MCL-Cartographer yaw divergence in radians</summary>
        public double MaxMclYawDivergence { get; init; } = 0.2;

        /// <summary>Threshold below which to flag potential drift (0.0-1.0)</summary>
        public double DriftWarningThreshold { get; init; } = 0.5;

        /// <summary>Threshold below which to flag critical drift (0.0-1.0)</summary>
        public double DriftCriticalThreshold { get; init; } = 0.3;

        /// <summary>
        /// Critical scan match score threshold for "veto" logic.
        /// When ScanMatchScore falls below this, CombinedScore is capped regardless of other metrics.
        /// This prevents other "good" metrics from masking a fundamental scan matching failure.
        /// </summary>
        public double ScanMatchVetoThreshold { get; init; } = 0.25;

        /// <summary>
        /// Maximum CombinedScore allowed when ScanMatchScore is below veto threshold.
        /// Even if other metrics are perfect, the score cannot exceed this cap.
        /// </summary>
        public double ScanMatchVetoCap { get; init; } = 0.35;

        /// <summary>
        /// Maximum allowed pose jump distance (meters) per update cycle.
        /// Jumps larger than this indicate teleportation or severe error.
        /// </summary>
        public double MaxPoseJumpDistance { get; init; } = 0.5;

        /// <summary>
        /// Maximum allowed pose jump rotation (radians) per update cycle.
        /// </summary>
        public double MaxPoseJumpRotation { get; init; } = 0.5;

        /// <summary>
        /// Scan match variance threshold to distinguish drift vs dynamic obstacles.
        /// High variance (> threshold) suggests dynamic obstacles.
        /// Low variance with low score suggests drift.
        /// </summary>
        public double ScanMatchVarianceThreshold { get; init; } = 0.04; // std dev ~0.2

        // Weights for combining different metrics
        public double ScanMatchWeight { get; init; } = 0.30;
        public double OdometryResidualWeight { get; init; } = 0.20;
        public double MclDivergenceWeight { get; init; } = 0.25;
        public double ConstraintQualityWeight { get; init; } = 0.15;
        public double CovarianceWeight { get; init; } = 0.10;
    }

    #endregion

    #region Metrics Result

    /// <summary>Result containing all drift detection metrics</summary>
    public record DriftMetrics
    {
        /// <summary>Raw scan match score from Cartographer (0.0-1.0)</summary>
        public double ScanMatchScore { get; init; }

        /// <summary>Normalized scan match score (0.0-1.0)</summary>
        public double ScanMatchScoreNormalized { get; init; }

        /// <summary>Odometry residual in meters (accumulated drift from odometry)</summary>
        public double OdometryResidual { get; init; }

        /// <summary>Normalized odometry residual score (0.0-1.0, higher is better)</summary>
        public double OdometryResidualScore { get; init; }

        /// <summary>Distance between MCL pose and Cartographer pose in meters</summary>
        public double? MclDivergence { get; init; }

        /// <summary>Yaw difference between MCL and Cartographer in radians</summary>
        public double? MclYawDivergence { get; init; }

        /// <summary>Normalized MCL divergence score (0.0-1.0, higher is better)</summary>
        public double MclDivergenceScore { get; init; }

        /// <summary>Constraint quality from pose graph (0.0-1.0)</summary>
        public double ConstraintQuality { get; init; }

        /// <summary>Covariance-based score (0.0-1.0)</summary>
        public double CovarianceScore { get; init; }

        /// <summary>Combined drift score (0.0-1.0, higher means more confident/less drift)</summary>
        public double CombinedScore { get; init; }

        /// <summary>Drift status based on combined score</summary>
        public DriftStatus Status { get; init; }

        /// <summary>Moving average of combined score over window</summary>
        public double MovingAverageScore { get; init; }

        /// <summary>Trend of score: positive = improving, negative = degrading</summary>
        public double ScoreTrend { get; init; }

        /// <summary>True if a sudden pose jump was detected (possible kidnapping or severe error)</summary>
        public bool PoseJumpDetected { get; init; }

        /// <summary>Distance of pose jump in meters (0 if no jump)</summary>
        public double PoseJumpDistance { get; init; }

        /// <summary>Variance of scan match scores over the window (high variance = dynamic obstacles)</summary>
        public double ScanMatchVariance { get; init; }

        /// <summary>
        /// Type of localization degradation detected.
        /// Helps distinguish between drift and dynamic obstacles.
        /// </summary>
        public DegradationType Degradation { get; init; }
    }

    /// <summary>Drift status levels</summary>
    public enum DriftStatus
    {
        /// <summary>Localization is stable and confident</summary>
        Stable,

        /// <summary>Minor degradation detected, monitoring</summary>
        Warning,

        /// <summary>Significant drift detected, may need relocalization</summary>
        Critical,

        /// <summary>Severe drift, relocalization recommended</summary>
        Lost
    }

    /// <summary>
    /// Type of localization degradation, helps distinguish root cause.
    /// </summary>
    public enum DegradationType
    {
        /// <summary>No degradation, localization is healthy</summary>
        None,

        /// <summary>
        /// Gradual drift detected: low scan match scores with low variance,
        /// increasing odometry residual over time. Robot position is slowly
        /// diverging from true position.
        /// </summary>
        Drift,

        /// <summary>
        /// Dynamic obstacles detected: high scan match variance (fluctuating scores),
        /// but odometry residual remains low. Temporary occlusion from moving objects.
        /// </summary>
        DynamicObstacles,

        /// <summary>
        /// Sudden pose jump detected: large position change in short time.
        /// Possible causes: robot kidnapping, scan matcher jumped to wrong location,
        /// or map ambiguity (similar-looking areas).
        /// </summary>
        PoseJump,

        /// <summary>
        /// Featureless area: low scan match scores due to lack of distinctive features.
        /// Common in long corridors or open spaces.
        /// </summary>
        FeaturelessArea,

        /// <summary>
        /// Unknown degradation: cannot determine specific cause.
        /// </summary>
        Unknown
    }

    #endregion

    #region Fields

    private readonly DriftDetectorConfig _config;
    private readonly Lock _lock = new();

    // Moving window for score history
    private readonly Queue<double> _scoreHistory;
    private readonly Queue<double> _scanMatchHistory;
    private readonly Queue<double> _odometryResidualHistory;

    // Odometry tracking for residual calculation
    private Pose _lastOdometryPose;
    private Pose _lastCartographerPose;
    private double _accumulatedOdometryDistance;
    private double _accumulatedCartographerDistance;
    private bool _initialized;

    // Pose jump detection
    private Pose _previousPoseForJumpDetection;
    private bool _poseJumpInitialized;

    // Latest metrics
    private DriftMetrics _latestMetrics = new()
    {
        ScanMatchScore = 1.0,
        ScanMatchScoreNormalized = 1.0,
        OdometryResidual = 0.0,
        OdometryResidualScore = 1.0,
        MclDivergenceScore = 1.0,
        ConstraintQuality = 1.0,
        CovarianceScore = 1.0,
        CombinedScore = 1.0,
        Status = DriftStatus.Stable,
        MovingAverageScore = 1.0,
        ScoreTrend = 0.0,
        PoseJumpDetected = false,
        PoseJumpDistance = 0.0,
        ScanMatchVariance = 0.0,
        Degradation = DegradationType.None
    };

    #endregion

    #region Constructor

    public DriftDetector(DriftDetectorConfig? config = null)
    {
        _config = config ?? new DriftDetectorConfig();
        _scoreHistory = new Queue<double>(_config.WindowSize);
        _scanMatchHistory = new Queue<double>(_config.WindowSize);
        _odometryResidualHistory = new Queue<double>(_config.WindowSize);
    }

    #endregion

    #region Public Methods

    /// <summary>
    /// Update drift detection with new sensor data.
    /// Call this method each time new localization data is available.
    /// </summary>
    /// <param name="cartographerPose">Current pose from Cartographer</param>
    /// <param name="odometryPose">Current pose from odometry (optional)</param>
    /// <param name="scanMatchScore">Scan match confidence from Cartographer (PoseConfidence)</param>
    /// <param name="covariance">Pose covariance matrix (optional)</param>
    /// <param name="constraintCount">Number of constraints in pose graph</param>
    /// <param name="constraintQuality">Average constraint quality (0.0-1.0)</param>
    /// <param name="mclPose">Pose from MCL if running in parallel (optional)</param>
    /// <param name="mclReliability">MCL reliability score (optional)</param>
    /// <returns>Updated drift metrics</returns>
    public DriftMetrics Update(
        Pose cartographerPose,
        Pose? odometryPose,
        double scanMatchScore,
        Matrix3x3? covariance,
        int constraintCount,
        double constraintQuality,
        Pose? mclPose = null,
        double? mclReliability = null)
    {
        lock (_lock)
        {
            // 1. Calculate scan match score (normalized)
            var scanMatchScoreNormalized = NormalizeScanMatchScore(scanMatchScore);
            AddToHistory(_scanMatchHistory, scanMatchScoreNormalized);

            // 2. Calculate odometry residual
            double odometryResidual = 0.0;
            double odometryResidualScore = 1.0;

            if (odometryPose.HasValue)
            {
                odometryResidual = CalculateOdometryResidual(cartographerPose, odometryPose.Value);
                odometryResidualScore = CalculateOdometryResidualScore(odometryResidual);
                AddToHistory(_odometryResidualHistory, odometryResidual);
            }

            // 3. Detect pose jump (sudden large position change)
            var (poseJumpDetected, poseJumpDistance) = DetectPoseJump(cartographerPose);

            // 4. Calculate MCL divergence (if MCL pose available)
            double? mclDivergence = null;
            double? mclYawDivergence = null;
            double mclDivergenceScore = 0.5; // Neutral if no MCL

            if (mclPose.HasValue)
            {
                (mclDivergence, mclYawDivergence) = CalculateMclDivergence(cartographerPose, mclPose.Value);
                mclDivergenceScore = CalculateMclDivergenceScore(mclDivergence.Value, mclYawDivergence.Value, mclReliability);
            }

            // 5. Calculate covariance score
            var covarianceScore = CalculateCovarianceScore(covariance);

            // 6. Calculate combined score with adaptive weights
            var weights = CalculateAdaptiveWeights(
                hasMcl: mclPose.HasValue,
                hasOdometry: odometryPose.HasValue,
                constraintCount: constraintCount);

            var combinedScore =
                (scanMatchScoreNormalized * weights.ScanMatch) +
                (odometryResidualScore * weights.OdometryResidual) +
                (mclDivergenceScore * weights.MclDivergence) +
                (constraintQuality * weights.ConstraintQuality) +
                (covarianceScore * weights.Covariance);

            // 6b. Apply "veto" logic: if ScanMatchScore is critically low, cap CombinedScore
            // This prevents other "good" metrics from masking a fundamental scan matching failure.
            // Rationale: When scan matching fails, covariance/constraints computed from bad matches
            // are unreliable, so their high values shouldn't override the scan match warning.
            if (scanMatchScoreNormalized < _config.ScanMatchVetoThreshold)
            {
                combinedScore = Math.Min(combinedScore, _config.ScanMatchVetoCap);
            }

            // 6c. If pose jump detected, cap score severely (possible kidnapping)
            if (poseJumpDetected)
            {
                combinedScore = Math.Min(combinedScore, 0.2);
            }

            combinedScore = Math.Clamp(combinedScore, 0.0, 1.0);

            // 7. Update score history and calculate moving average
            AddToHistory(_scoreHistory, combinedScore);
            var movingAverage = _scoreHistory.Count > 0 ? _scoreHistory.Average() : combinedScore;

            // 8. Calculate trend (positive = improving, negative = degrading)
            var trend = CalculateTrend();

            // 9. Calculate scan match variance (helps distinguish drift vs dynamic obstacles)
            var scanMatchVariance = CalculateScanMatchVariance();

            // 10. Determine drift status (pass scanMatchScoreNormalized for veto logic)
            var status = DetermineDriftStatus(movingAverage, trend, scanMatchScoreNormalized);

            // 10b. If pose jump detected, force Critical status
            if (poseJumpDetected && status < DriftStatus.Critical)
            {
                status = DriftStatus.Critical;
            }

            // 11. Determine degradation type (drift vs dynamic obstacles vs pose jump)
            var degradationType = DetermineDegradationType(
                status,
                scanMatchScoreNormalized,
                scanMatchVariance,
                odometryResidual,
                poseJumpDetected,
                constraintCount);

            // 12. Build result
            _latestMetrics = new DriftMetrics
            {
                ScanMatchScore = scanMatchScore,
                ScanMatchScoreNormalized = scanMatchScoreNormalized,
                OdometryResidual = odometryResidual,
                OdometryResidualScore = odometryResidualScore,
                MclDivergence = mclDivergence,
                MclYawDivergence = mclYawDivergence,
                MclDivergenceScore = mclDivergenceScore,
                ConstraintQuality = constraintQuality,
                CovarianceScore = covarianceScore,
                CombinedScore = combinedScore,
                Status = status,
                MovingAverageScore = movingAverage,
                ScoreTrend = trend,
                PoseJumpDetected = poseJumpDetected,
                PoseJumpDistance = poseJumpDistance,
                ScanMatchVariance = scanMatchVariance,
                Degradation = degradationType
            };

            return _latestMetrics;
        }
    }

    /// <summary>Gets the latest drift metrics without updating</summary>
    public DriftMetrics GetLatestMetrics()
    {
        lock (_lock)
        {
            return _latestMetrics;
        }
    }

    /// <summary>Resets the drift detector state</summary>
    public void Reset()
    {
        lock (_lock)
        {
            _scoreHistory.Clear();
            _scanMatchHistory.Clear();
            _odometryResidualHistory.Clear();
            _initialized = false;
            _poseJumpInitialized = false;
            _accumulatedOdometryDistance = 0;
            _accumulatedCartographerDistance = 0;
            _latestMetrics = new DriftMetrics
            {
                ScanMatchScore = 1.0,
                ScanMatchScoreNormalized = 1.0,
                OdometryResidual = 0.0,
                OdometryResidualScore = 1.0,
                MclDivergenceScore = 1.0,
                ConstraintQuality = 1.0,
                CovarianceScore = 1.0,
                CombinedScore = 1.0,
                Status = DriftStatus.Stable,
                MovingAverageScore = 1.0,
                ScoreTrend = 0.0,
                PoseJumpDetected = false,
                PoseJumpDistance = 0.0,
                ScanMatchVariance = 0.0,
                Degradation = DegradationType.None
            };
        }
    }

    #endregion

    #region Private Methods

    private static double NormalizeScanMatchScore(double rawScore)
    {
        // PoseConfidence from Cartographer is returned as percentage (0-100),
        // not as a normalized value (0-1). Handle both ranges.
        if (rawScore < 0) return 0.0;

        // Normalize to [0, 1] range if input is in [0, 100] range
        var score = rawScore;
        if (score > 1.0)
        {
            score = score / 100.0;
        }

        // Clamp to valid range
        return Math.Clamp(score, 0.0, 1.0);
    }

    /// <summary>
    /// Detects sudden large pose changes (teleportation or severe localization error).
    /// </summary>
    /// <returns>Tuple of (jumpDetected, jumpDistance)</returns>
    private (bool Detected, double Distance) DetectPoseJump(Pose currentPose)
    {
        if (!_poseJumpInitialized)
        {
            _previousPoseForJumpDetection = currentPose;
            _poseJumpInitialized = true;
            return (false, 0.0);
        }

        // Calculate position change
        var dx = currentPose.Position.X - _previousPoseForJumpDetection.Position.X;
        var dy = currentPose.Position.Y - _previousPoseForJumpDetection.Position.Y;
        var distance = Math.Sqrt(dx * dx + dy * dy);

        // Calculate rotation change (normalize to [-π, π])
        // Extract yaw from quaternion orientation
        var currentYaw = currentPose.Orientation.ToYawRadian();
        var previousYaw = _previousPoseForJumpDetection.Orientation.ToYawRadian();
        var dyaw = currentYaw - previousYaw;
        while (dyaw > Math.PI) dyaw -= 2 * Math.PI;
        while (dyaw < -Math.PI) dyaw += 2 * Math.PI;
        var rotationChange = Math.Abs(dyaw);

        // Update previous pose
        _previousPoseForJumpDetection = currentPose;

        // Check for jump
        bool isJump = distance > _config.MaxPoseJumpDistance ||
                      rotationChange > _config.MaxPoseJumpRotation;

        return (isJump, distance);
    }

    /// <summary>
    /// Calculates variance of scan match scores over the history window.
    /// High variance indicates fluctuating scores (likely dynamic obstacles).
    /// Low variance with low mean indicates consistent poor matching (likely drift).
    /// </summary>
    private double CalculateScanMatchVariance()
    {
        if (_scanMatchHistory.Count < 2)
            return 0.0;

        var mean = _scanMatchHistory.Average();
        var sumSquaredDiff = _scanMatchHistory.Sum(x => Math.Pow(x - mean, 2));
        return sumSquaredDiff / _scanMatchHistory.Count;
    }

    /// <summary>
    /// Determines the type of localization degradation based on multiple metrics.
    /// This helps users understand the root cause of localization issues.
    /// </summary>
    private DegradationType DetermineDegradationType(
        DriftStatus status,
        double scanMatchScore,
        double scanMatchVariance,
        double odometryResidual,
        bool poseJumpDetected,
        int constraintCount)
    {
        // If localization is stable, no degradation
        if (status == DriftStatus.Stable)
            return DegradationType.None;

        // Pose jump takes priority - it's a clear signal
        if (poseJumpDetected)
            return DegradationType.PoseJump;

        // High variance in scan match scores suggests dynamic obstacles
        // (scores fluctuate as obstacles move in and out of view)
        bool highVariance = scanMatchVariance > _config.ScanMatchVarianceThreshold;

        // Low odometry residual means odometry and Cartographer agree on movement
        // High residual means they disagree (accumulated drift)
        bool lowOdometryResidual = odometryResidual < _config.MaxOdometryResidual * 0.5;

        // Low scan match with high variance + low odometry residual = dynamic obstacles
        // Robot is in the right place, but moving objects are confusing the scan matcher
        if (highVariance && lowOdometryResidual)
            return DegradationType.DynamicObstacles;

        // Low scan match with low variance + increasing odometry residual = drift
        // Scan matcher consistently can't match well, and position is drifting
        if (!highVariance && !lowOdometryResidual)
            return DegradationType.Drift;

        // Low scan match with low variance + low odometry residual = featureless area
        // Not enough distinctive features for reliable matching, but robot hasn't moved much
        if (!highVariance && lowOdometryResidual && constraintCount < 5)
            return DegradationType.FeaturelessArea;

        // Low scan match but with some variance, could be drift beginning
        if (!highVariance && scanMatchScore < _config.MinScanMatchScore)
            return DegradationType.Drift;

        return DegradationType.Unknown;
    }

    private double CalculateOdometryResidual(Pose cartographerPose, Pose odometryPose)
    {
        if (!_initialized)
        {
            _lastOdometryPose = odometryPose;
            _lastCartographerPose = cartographerPose;
            _accumulatedOdometryDistance = 0;
            _accumulatedCartographerDistance = 0;
            _initialized = true;
            return 0.0;
        }

        // Calculate distance traveled according to odometry
        var odomDelta = Math.Sqrt(
            Math.Pow(odometryPose.Position.X - _lastOdometryPose.Position.X, 2) +
            Math.Pow(odometryPose.Position.Y - _lastOdometryPose.Position.Y, 2));

        // Calculate distance traveled according to Cartographer
        var cartoDelta = Math.Sqrt(
            Math.Pow(cartographerPose.Position.X - _lastCartographerPose.Position.X, 2) +
            Math.Pow(cartographerPose.Position.Y - _lastCartographerPose.Position.Y, 2));

        _accumulatedOdometryDistance += odomDelta;
        _accumulatedCartographerDistance += cartoDelta;

        // Update last poses
        _lastOdometryPose = odometryPose;
        _lastCartographerPose = cartographerPose;

        // Calculate residual as absolute difference in accumulated distances
        // This indicates drift between odometry and SLAM
        var residual = Math.Abs(_accumulatedOdometryDistance - _accumulatedCartographerDistance);

        // Reset accumulated distances periodically to avoid unbounded growth
        if (_accumulatedOdometryDistance > 10.0 || _accumulatedCartographerDistance > 10.0)
        {
            _accumulatedOdometryDistance = 0;
            _accumulatedCartographerDistance = 0;
        }

        return residual;
    }

    private double CalculateOdometryResidualScore(double residual)
    {
        // Convert residual to score: lower residual = higher score
        // Use exponential decay: score = exp(-k * residual)
        // k chosen so that MaxOdometryResidual gives ~0.37 (1/e)
        double k = 1.0 / _config.MaxOdometryResidual;
        return Math.Exp(-k * residual);
    }

    private (double distance, double yawDiff) CalculateMclDivergence(Pose cartographerPose, Pose mclPose)
    {
        // Calculate Euclidean distance between poses
        var distance = Math.Sqrt(
            Math.Pow(cartographerPose.Position.X - mclPose.Position.X, 2) +
            Math.Pow(cartographerPose.Position.Y - mclPose.Position.Y, 2));

        // Calculate yaw difference
        var cartoYaw = cartographerPose.Orientation.ToYawRadian();
        var mclYaw = mclPose.Orientation.ToYawRadian();
        var yawDiff = Math.Abs(NormalizeAngle(cartoYaw - mclYaw));

        return (distance, yawDiff);
    }

    private double CalculateMclDivergenceScore(double distance, double yawDiff, double? mclReliability)
    {
        // Distance score: exponential decay
        double distanceScore = Math.Exp(-distance / _config.MaxMclDivergence);

        // Yaw score: exponential decay
        double yawScore = Math.Exp(-yawDiff / _config.MaxMclYawDivergence);

        // Combine distance and yaw scores
        double geometricScore = (distanceScore * 0.7) + (yawScore * 0.3);

        // Weight by MCL reliability if available
        if (mclReliability.HasValue)
        {
            // If MCL is reliable and diverges from Cartographer, that's a strong signal
            // If MCL is unreliable, don't trust the divergence as much
            return geometricScore * (0.5 + 0.5 * mclReliability.Value);
        }

        return geometricScore;
    }

    private double CalculateCovarianceScore(Matrix3x3? covariance)
    {
        if (!covariance.HasValue)
            return 0.5; // Neutral if no covariance

        var cov = covariance.Value;
        var trace = cov[0, 0] + cov[1, 1] + cov[2, 2];

        if (trace < 1e-6)
            return 1.0; // Very low covariance = high confidence

        // Normalize: score = 1 / (1 + trace)
        return 1.0 / (1.0 + trace);
    }

    private record struct WeightSet(
        double ScanMatch,
        double OdometryResidual,
        double MclDivergence,
        double ConstraintQuality,
        double Covariance);

    private WeightSet CalculateAdaptiveWeights(bool hasMcl, bool hasOdometry, int constraintCount)
    {
        // Start with configured weights
        double scanMatch = _config.ScanMatchWeight;
        double odometry = _config.OdometryResidualWeight;
        double mcl = _config.MclDivergenceWeight;
        double constraint = _config.ConstraintQualityWeight;
        double covariance = _config.CovarianceWeight;

        // Redistribute MCL weight if not available
        if (!hasMcl)
        {
            // Give MCL weight to scan match (most reliable alternative)
            scanMatch += mcl * 0.6;
            odometry += mcl * 0.2;
            constraint += mcl * 0.2;
            mcl = 0;
        }

        // Redistribute odometry weight if not available
        if (!hasOdometry)
        {
            scanMatch += odometry * 0.5;
            constraint += odometry * 0.3;
            covariance += odometry * 0.2;
            odometry = 0;
        }

        // Reduce constraint weight if few constraints
        if (constraintCount < 5)
        {
            double reduction = constraint * 0.5;
            constraint *= 0.5;
            scanMatch += reduction;
        }

        // Normalize to sum to 1.0
        double total = scanMatch + odometry + mcl + constraint + covariance;
        if (total > 0)
        {
            scanMatch /= total;
            odometry /= total;
            mcl /= total;
            constraint /= total;
            covariance /= total;
        }

        return new WeightSet(scanMatch, odometry, mcl, constraint, covariance);
    }

    private double CalculateTrend()
    {
        if (_scoreHistory.Count < 3)
            return 0.0;

        var scores = _scoreHistory.ToArray();
        int n = scores.Length;

        // Calculate simple linear trend using least squares
        // trend = (n * sum(i*y[i]) - sum(i) * sum(y[i])) / (n * sum(i^2) - sum(i)^2)
        double sumI = 0, sumY = 0, sumIY = 0, sumI2 = 0;
        for (int i = 0; i < n; i++)
        {
            sumI += i;
            sumY += scores[i];
            sumIY += i * scores[i];
            sumI2 += i * i;
        }

        double denominator = n * sumI2 - sumI * sumI;
        if (Math.Abs(denominator) < 1e-10)
            return 0.0;

        double trend = (n * sumIY - sumI * sumY) / denominator;

        // Normalize trend to roughly [-1, 1] range
        // Multiply by window size to make it scale-independent
        return trend * n;
    }

    private DriftStatus DetermineDriftStatus(double movingAverage, double trend, double scanMatchScoreNormalized)
    {
        // Veto logic: if ScanMatchScore is critically low, force at least Warning status
        // regardless of combined score. This ensures scan matching failures are never masked.
        DriftStatus minStatus = DriftStatus.Stable;
        if (scanMatchScoreNormalized < _config.ScanMatchVetoThreshold)
        {
            // Scan matching is failing - at minimum this is Critical
            minStatus = DriftStatus.Critical;
        }
        else if (scanMatchScoreNormalized < _config.MinScanMatchScore)
        {
            // Scan matching is poor - at minimum this is Warning
            minStatus = DriftStatus.Warning;
        }

        // Consider both current score and trend
        double effectiveScore = movingAverage;

        // If score is declining rapidly, be more aggressive
        if (trend < -0.1)
        {
            effectiveScore -= 0.1;
        }

        DriftStatus computedStatus;
        if (effectiveScore < 0.15)
            computedStatus = DriftStatus.Lost;
        else if (effectiveScore < _config.DriftCriticalThreshold)
            computedStatus = DriftStatus.Critical;
        else if (effectiveScore < _config.DriftWarningThreshold)
            computedStatus = DriftStatus.Warning;
        else
            computedStatus = DriftStatus.Stable;

        // Return the worse of computed status and veto-enforced minimum status
        // DriftStatus enum: Stable=0, Warning=1, Critical=2, Lost=3
        return (DriftStatus)Math.Max((int)computedStatus, (int)minStatus);
    }

    private void AddToHistory(Queue<double> history, double value)
    {
        if (history.Count >= _config.WindowSize)
        {
            history.Dequeue();
        }
        history.Enqueue(value);
    }

    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }

    #endregion
}