BQP/srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Services/MetricsCalculatorTests.cs

using FluentAssertions;
using Xunit;
using RobotNet10.NavigationTune.Shared.Models;
using RobotNet10.NavigationTune.Services;
using RobotNet10.NavigationTune.Test.Helpers;

namespace RobotNet10.NavigationTune.Test.Services;

public class MetricsCalculatorTests
{
    private readonly MetricsCalculator _calculator;

    public MetricsCalculatorTests()
    {
        _calculator = new MetricsCalculator();
    }

    [Fact]
    public void CalculateMetrics_WithEmptyTelemetry_ShouldThrowException()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act & Assert
        var action = () => _calculator.CalculateMetrics(telemetry, referencePath);
        action.Should().Throw<ArgumentException>()
            .WithMessage("Telemetry data cannot be empty*");
    }

    [Fact]
    public void CalculateMetrics_WithPerfectTracking_ShouldReturnHighScores()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Create perfect tracking data (no errors)
        for (int i = 0; i < 100; i++)
        {
            telemetry.Add(TestHelpers.CreateTelemetryData(
                x: i * 0.1,
                y: 0.0,
                theta: 0.0,
                linearVel: 1.0,
                angularVel: 0.0,
                cte: 0.0,
                headingError: 0.0
            ));
        }

        // Act
        var result = _calculator.CalculateMetrics(telemetry, referencePath);

        // Assert
        result.Should().NotBeNull();
        result!.CrossTrackErrorRMS.Should().BeApproximately(0.0, 0.01f);
        result.HeadingErrorRMS.Should().BeApproximately(0.0, 0.01f);
        result.TrackingScore.Should().BeGreaterThan(90.0);
    }

    [Fact]
    public void CalculateMetrics_WithTrackingErrors_ShouldCalculateCorrectRMS()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Create data with constant CTE error
        const double constantCTE = 0.1;
        for (int i = 0; i < 100; i++)
        {
            telemetry.Add(TestHelpers.CreateTelemetryData(
                x: i * 0.1,
                y: constantCTE, // Constant offset
                theta: 0.0,
                linearVel: 1.0,
                angularVel: 0.0,
                cte: constantCTE,
                headingError: 0.0
            ));
        }

        // Act
        var result = _calculator.CalculateMetrics(telemetry, referencePath);

        // Assert
        result.Should().NotBeNull();
        result!.CrossTrackErrorRMS.Should().BeApproximately(constantCTE, 0.01f);
        result.CrossTrackErrorMean.Should().BeApproximately(constantCTE, 0.01f);
    }

    [Fact]
    public void CalculateMetrics_ShouldCalculateSmoothnessMetrics()
    {
        // Arrange
        // Create telemetry with varying velocities to test smoothness calculation
        var telemetry = new List<TelemetryData>();
        for (int i = 0; i < 100; i++)
        {
            telemetry.Add(TestHelpers.CreateTelemetryData(
                x: i * 0.1,
                y: 0.0,
                theta: 0.0,
                linearVel: 1.0 + Math.Sin(i * 0.1) * 0.2, // Varying velocity
                angularVel: 0.0,
                cte: 0.0,
                headingError: 0.0
            ));
        }
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act
        var result = _calculator.CalculateMetrics(telemetry, referencePath);

        // Assert
        result.Should().NotBeNull();
        result!.VelocityStdDev.Should().BeGreaterThanOrEqualTo(0.0);
        result.AccelerationStdDev.Should().BeGreaterThanOrEqualTo(0.0);
    }

    [Fact]
    public void CalculateMetrics_ShouldCalculateEfficiencyMetrics()
    {
        // Arrange
        // Create telemetry with proper timestamps
        var telemetry = new List<TelemetryData>();
        var startTime = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
        for (int i = 0; i < 100; i++)
        {
            telemetry.Add(new TelemetryData
            {
                TimestampMs = startTime + i * 20, // 20ms intervals = 2 seconds total
                RobotPose = new Pose2D(i * 0.1, 0.0, 0.0),
                RobotTwist = new Twist2D(1.0, 0.0),
                ReferencePose = new Pose2D(i * 0.1, 0.0, 0.0),
                CrossTrackError = 0.0,
                HeadingError = 0.0,
                LookaheadDistance = 1.0,
                ModelConfidence = 1.0,
                DistanceToGoal = (100 - i) * 0.1
            });
        }
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act
        var result = _calculator.CalculateMetrics(telemetry, referencePath);

        // Assert
        result.Should().NotBeNull();
        result!.PathLengthRatio.Should().BeGreaterThan(0.0);
        // CompletionTime = (last timestamp - first timestamp) / 1000
        result.CompletionTime.Should().BeGreaterThanOrEqualTo(0.0);
        result.AverageSpeed.Should().BeGreaterThan(0.0);
        result.MaxSpeed.Should().BeGreaterThan(0.0);
    }

    [Fact]
    public void CalculateMetrics_ShouldCalculateOverallScore()
    {
        // Arrange
        // Create telemetry with proper timestamps and varying data
        var telemetry = new List<TelemetryData>();
        var startTime = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
        for (int i = 0; i < 100; i++)
        {
            telemetry.Add(new TelemetryData
            {
                TimestampMs = startTime + i * 20,
                RobotPose = new Pose2D(i * 0.1, 0.0, 0.0),
                RobotTwist = new Twist2D(1.0 + Math.Sin(i * 0.1) * 0.1, 0.0),
                ReferencePose = new Pose2D(i * 0.1, 0.0, 0.0),
                CrossTrackError = 0.05f,
                HeadingError = 0.01f,
                LookaheadDistance = 1.0,
                ModelConfidence = 1.0,
                DistanceToGoal = (100 - i) * 0.1
            });
        }
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act
        var result = _calculator.CalculateMetrics(telemetry, referencePath);

        // Assert
        result.Should().NotBeNull();
        // Check if scores are valid (not NaN or Infinity)
        if (!double.IsNaN(result!.OverallScore) && !double.IsInfinity(result.OverallScore))
        {
            result.OverallScore.Should().BeInRange(0.0, 100.0);
        }
        if (!double.IsNaN(result.TrackingScore) && !double.IsInfinity(result.TrackingScore))
        {
            result.TrackingScore.Should().BeInRange(0.0, 100.0);
        }
        if (!double.IsNaN(result.SmoothnessScore) && !double.IsInfinity(result.SmoothnessScore))
        {
            result.SmoothnessScore.Should().BeInRange(0.0, 100.0);
        }
        if (!double.IsNaN(result.EfficiencyScore) && !double.IsInfinity(result.EfficiencyScore))
        {
            result.EfficiencyScore.Should().BeInRange(0.0, 100.0);
        }
    }

    [Fact]
    public void CalculateMetrics_WithGoalReached_ShouldCalculateGoalErrors()
    {
        // Arrange
        var telemetry = new List<TelemetryData>();
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Create data ending at goal
        for (int i = 0; i < 100; i++)
        {
            telemetry.Add(TestHelpers.CreateTelemetryData(
                x: i * 0.1,
                y: 0.0,
                theta: 0.0,
                linearVel: 1.0,
                angularVel: 0.0,
                cte: 0.0,
                headingError: 0.0
            ));
        }

        // Act
        var result = _calculator.CalculateMetrics(telemetry, referencePath);

        // Assert
        result.Should().NotBeNull();
        result!.GoalPositionError.Should().BeGreaterThanOrEqualTo(0.0);
        result.GoalHeadingError.Should().BeGreaterThanOrEqualTo(0.0);
    }
}