BQP/srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Execution/TestExecutorTests.cs

using FluentAssertions;
using Moq;
using Xunit;
using RobotNet10.NavigationTune.Execution;
using RobotNet10.NavigationTune.Shared.Interfaces;
using RobotNet10.NavigationTune.Shared.Models;
using RobotNet10.NavigationTune.Test.Helpers;
using RobotNet10.NavigationTune.Scenarios;

namespace RobotNet10.NavigationTune.Test.Execution;

public class TestExecutorTests
{
    private Mock<ILocalizationProvider> CreateMockLocalization(double x = 0.0, double y = 0.0, double theta = 0.0)
    {
        var mock = new Mock<ILocalizationProvider>();
        mock.Setup(l => l.X).Returns(x);
        mock.Setup(l => l.Y).Returns(y);
        mock.Setup(l => l.Theta).Returns(theta);
        return mock;
    }

    private Mock<IVelocityProvider> CreateMockVelocityProvider(
        double linearVel = 0.0, 
        double angularVel = 0.0,
        double confidence = 1.0)
    {
        var mock = new Mock<IVelocityProvider>();
        mock.Setup(v => v.GetActualVelocity()).Returns((linearVel, angularVel));
        mock.Setup(v => v.GetModelConfidence()).Returns(confidence);
        return mock;
    }

    [Fact]
    public void Constructor_WithProviders_ShouldInitialize()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();

        // Act
        var executor = new TestExecutor(
            localization.Object,
            velocityProvider.Object);

        // Assert
        executor.Should().NotBeNull();
    }

    [Fact]
    public async Task ExecuteAsync_WithStraightLineScenario_ShouldComplete()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateStraightLineScenario(5.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        var telemetryUpdates = new List<TelemetryData>();

        // Act
        var result = await executor.ExecuteAsync(
            scenario,
            parameters,
            telemetry => telemetryUpdates.Add(telemetry),
            new CancellationTokenSource(TimeSpan.FromSeconds(1)).Token);

        // Assert
        result.Should().NotBeNull();
        result.Status.Should().BeOneOf(TestStatus.Completed, TestStatus.Aborted);
        telemetryUpdates.Should().NotBeEmpty();
    }

    [Fact]
    public async Task ExecuteAsync_WhenCancelled_ShouldAbort()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateStraightLineScenario(10.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();
        var cts = new CancellationTokenSource(TimeSpan.FromMilliseconds(100));

        // Act
        var result = await executor.ExecuteAsync(scenario, parameters, null, cts.Token);

        // Assert
        result.Status.Should().BeOneOf(TestStatus.Aborted, TestStatus.Error);
    }

    [Fact]
    public void GetProgress_BeforeExecution_ShouldReturnZero()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        // Act
        var progress = executor.GetProgress();

        // Assert
        progress.ProgressPercent.Should().Be(0.0);
        progress.DistanceTraveled.Should().Be(0.0);
    }

    [Fact]
    public async Task Pause_WhenRunning_ShouldChangeStatus()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateStraightLineScenario(1.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act - Start test in background
        var task = Task.Run(async () =>
        {
            await executor.ExecuteAsync(scenario, parameters, null, 
                new CancellationTokenSource(TimeSpan.FromSeconds(2)).Token);
        });

        // Wait a bit then pause
        await Task.Delay(100);
        executor.Pause();

        // Assert
        var progress = executor.GetProgress();
        progress.Should().NotBeNull();

        // Cleanup
        executor.Stop();
        await task;
    }

    [Fact]
    public async Task Resume_AfterPause_ShouldContinue()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateStraightLineScenario(1.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var task = Task.Run(async () =>
        {
            await executor.ExecuteAsync(scenario, parameters, null,
                new CancellationTokenSource(TimeSpan.FromSeconds(2)).Token);
        });

        await Task.Delay(100);
        executor.Pause();
        await Task.Delay(50);
        executor.Resume();

        // Assert
        var progress = executor.GetProgress();
        progress.Should().NotBeNull();

        // Cleanup
        executor.Stop();
        await task;
    }

    [Fact]
    public async Task Stop_WhenRunning_ShouldAbort()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateStraightLineScenario(10.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var task = Task.Run(async () =>
        {
            await executor.ExecuteAsync(scenario, parameters, null,
                new CancellationTokenSource(TimeSpan.FromSeconds(5)).Token);
        });

        await Task.Delay(200);
        executor.Stop();

        // Assert
        await task;
        var progress = executor.GetProgress();
        progress.Should().NotBeNull();
    }

    [Fact]
    public async Task EmergencyStop_WhenRunning_ShouldImmediatelyStop()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateStraightLineScenario(10.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var task = Task.Run(async () =>
        {
            await executor.ExecuteAsync(scenario, parameters, null,
                new CancellationTokenSource(TimeSpan.FromSeconds(5)).Token);
        });

        await Task.Delay(200);
        executor.EmergencyStop();

        // Assert
        await task;
        var progress = executor.GetProgress();
        progress.Should().NotBeNull();
    }

    [Fact]
    public async Task ExecuteAsync_WithCircleScenario_ShouldComplete()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateCircleScenario(2.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        // Act
        var result = await executor.ExecuteAsync(
            scenario,
            parameters,
            null,
            new CancellationTokenSource(TimeSpan.FromSeconds(1)).Token);

        // Assert
        result.Should().NotBeNull();
        result.Status.Should().BeOneOf(TestStatus.Completed, TestStatus.Aborted);
    }

    [Fact]
    public async Task ExecuteAsync_ShouldCollectTelemetry()
    {
        // Arrange
        var localization = CreateMockLocalization();
        var velocityProvider = CreateMockVelocityProvider();
        var executor = new TestExecutor(localization.Object, velocityProvider.Object);

        var scenario = TestHelpers.CreateStraightLineScenario(2.0);
        var parameters = TestHelpers.CreateDefaultParameterSet();

        var telemetryList = new List<TelemetryData>();

        // Act
        var result = await executor.ExecuteAsync(
            scenario,
            parameters,
            telemetry => telemetryList.Add(telemetry),
            new CancellationTokenSource(TimeSpan.FromSeconds(1)).Token);

        // Assert
        telemetryList.Should().NotBeEmpty();
        result.TelemetryData.Should().NotBeEmpty();
    }
}