BQP/srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Services/SafetyMonitorTests.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 SafetyMonitorTests
{
    private readonly SafetyMonitor _safetyMonitor;
    private readonly SafetyConfig _safetyConfig;

    public SafetyMonitorTests()
    {
        _safetyConfig = new SafetyConfig();
        _safetyMonitor = new SafetyMonitor(_safetyConfig);
    }

    [Fact]
    public void CheckSafety_WithLowCTE_ShouldReturnTrue()
    {
        // Arrange
        var telemetry = TestHelpers.CreateTelemetryData(cte: 0.05f);
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act
        var result = _safetyMonitor.CheckSafety(telemetry, referencePath);

        // Assert
        result.Should().BeTrue();
    }

    [Fact]
    public void CheckSafety_WithHighCTE_ShouldReturnFalse()
    {
        // Arrange
        var telemetry = TestHelpers.CreateTelemetryData(cte: 2.0); // Very high CTE
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act
        var result = _safetyMonitor.CheckSafety(telemetry, referencePath);

        // Assert
        result.Should().BeFalse();
    }

    [Fact]
    public void CheckSafety_WithHighHeadingError_ShouldReturnFalse()
    {
        // Arrange
        var telemetry = TestHelpers.CreateTelemetryData(headingError: 1.5); // Very high heading error
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act
        var result = _safetyMonitor.CheckSafety(telemetry, referencePath);

        // Assert
        result.Should().BeFalse();
    }

    [Fact]
    public void Reset_ShouldClearViolations()
    {
        // Arrange
        var telemetry = TestHelpers.CreateTelemetryData(cte: 2.0);
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };
        _safetyMonitor.CheckSafety(telemetry, referencePath);

        // Act
        _safetyMonitor.Reset();
        var violations = _safetyMonitor.GetViolations();

        // Assert
        violations.Should().BeEmpty();
    }

    [Fact]
    public void GetViolations_AfterSafetyCheck_ShouldReturnViolations()
    {
        // Arrange
        var telemetry = TestHelpers.CreateTelemetryData(cte: 2.0);
        var referencePath = new ReferencePath
        {
            Points = TestHelpers.CreateSimplePath(10, 10.0),
            TotalLength = 10.0
        };

        // Act
        _safetyMonitor.CheckSafety(telemetry, referencePath);
        var violations = _safetyMonitor.GetViolations();

        // Assert
        violations.Should().NotBeEmpty();
    }
}