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

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Helper class for calculating covariance from Cartographer constraints
/// </summary>
public static class CovarianceCalculator
{
    /// <summary>
    /// Result of covariance calculation
    /// </summary>
    public record Result(
        Matrix3x3? Covariance,
        int ConstraintCount,
        double AverageConstraintQuality);

    /// <summary>
    /// Calculate covariance and quality metrics from constraints
    /// </summary>
    /// <param name="constraints">List of pose graph constraints</param>
    /// <returns>Covariance calculation result</returns>
    public static Result Calculate(IList<IPoseGraph.Constraint> constraints)
    {
        if (constraints == null || constraints.Count == 0)
        {
            return new Result(null, 0, 0.0);
        }

        int constraintCount = constraints.Count;
        
        // Calculate average constraint weights
        var avgTranslationWeight = constraints.Average(c => c.ConstraintPose.TranslationWeight);
        var avgRotationWeight = constraints.Average(c => c.ConstraintPose.RotationWeight);

        // Calculate constraint quality score (0.0 - 1.0)
        var normalizedTranslationQuality = Math.Min(1.0, avgTranslationWeight / 100.0);
        var normalizedRotationQuality = Math.Min(1.0, avgRotationWeight / 100.0);
        var averageConstraintQuality = (normalizedTranslationQuality + normalizedRotationQuality) / 2.0;

        // Calculate covariance from constraint weights
        var translationWeightVariance = constraints
            .Select(c => c.ConstraintPose.TranslationWeight)
            .Select(w => Math.Pow(w - avgTranslationWeight, 2))
            .Average();
            
        var rotationWeightVariance = constraints
            .Select(c => c.ConstraintPose.RotationWeight)
            .Select(w => Math.Pow(w - avgRotationWeight, 2))
            .Average();

        // Convert weights to covariance (inverse relationship)
        var translationCovBase = 1.0 / (avgTranslationWeight + 1e-6);
        var rotationCovBase = 1.0 / (avgRotationWeight + 1e-6);

        // Adjust covariance based on variance
        var translationVarianceFactor = 1.0 + (translationWeightVariance / (avgTranslationWeight * avgTranslationWeight + 1e-6));
        var rotationVarianceFactor = 1.0 + (rotationWeightVariance / (avgRotationWeight * avgRotationWeight + 1e-6));

        var translationCov = translationCovBase * translationVarianceFactor;
        var rotationCov = rotationCovBase * rotationVarianceFactor;

        // Adjust covariance based on number of constraints
        var constraintCountFactor = 1.0 / (1.0 + Math.Log10(Math.Max(1, constraintCount)));
        translationCov *= constraintCountFactor;
        rotationCov *= constraintCountFactor;

        // Create covariance matrix for 2D pose (x, y, theta)
        var covariance = Matrix3x3.Covariance(
            xx: translationCov,
            yy: translationCov,
            tt: rotationCov,
            xy: 0.0,
            xt: 0.0,
            yt: 0.0
        );

        return new Result(covariance, constraintCount, averageConstraintQuality);
    }
}