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

namespace RobotNet10.NavigationTune.Test.Scenarios;

public class StraightLineScenarioTests
{
    [Fact]
    public void GenerateReferencePath_ShouldGenerateStraightPath()
    {
        // Arrange
        var scenario = TestHelpers.CreateStraightLineScenario(10.0);

        // Act
        var path = scenario.GenerateReferencePath();

        // Assert
        path.Should().NotBeNull();
        path.Count.Should().BeGreaterThan(0);
        
        // First point should be at origin
        path[0].X.Should().BeApproximately(0.0, 0.01);
        path[0].Y.Should().BeApproximately(0.0, 0.01);
        
        // Last point should be at length
        path[^1].X.Should().BeApproximately(10.0, 0.01);
        path[^1].Y.Should().BeApproximately(0.0, 0.01);
    }

    [Fact]
    public void GenerateReferencePath_ShouldHaveCorrectDirection()
    {
        // Arrange
        var scenario = TestHelpers.CreateStraightLineScenario(10.0);

        // Act
        var path = scenario.GenerateReferencePath();

        // Assert
        // All points should have FORWARD direction (straight line)
        foreach (var point in path)
        {
            point.Direction.Should().Be(RobotDirection.FORWARD);
        }
    }

    [Fact]
    public void IsGoalReached_WithPoseAtGoal_ShouldReturnTrue()
    {
        // Arrange
        var scenario = TestHelpers.CreateStraightLineScenario(10.0);
        var goalPose = scenario.GetGoalPose();
        var poseAtGoal = new Pose2D(goalPose.X, goalPose.Y, goalPose.Theta);

        // Act
        var result = scenario.IsGoalReached(poseAtGoal);

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

    [Fact]
    public void IsGoalReached_WithPoseFarFromGoal_ShouldReturnFalse()
    {
        // Arrange
        var scenario = TestHelpers.CreateStraightLineScenario(10.0);
        var poseFarAway = new Pose2D(0.0, 0.0, 0.0);

        // Act
        var result = scenario.IsGoalReached(poseFarAway);

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

    [Fact]
    public void GetGoalPose_ShouldReturnCorrectGoal()
    {
        // Arrange
        var scenario = TestHelpers.CreateStraightLineScenario(10.0);

        // Act
        var goalPose = scenario.GetGoalPose();

        // Assert
        goalPose.X.Should().BeApproximately(10.0, 0.01);
        goalPose.Y.Should().BeApproximately(0.0, 0.01);
        goalPose.Theta.Should().BeApproximately(0.0, 0.01);
    }

    [Fact]
    public void GenerateReferencePath_WithDifferentLengths_ShouldGenerateCorrectPaths()
    {
        // Arrange
        var scenario1 = TestHelpers.CreateStraightLineScenario(5.0);
        var scenario2 = TestHelpers.CreateStraightLineScenario(20.0);

        // Act
        var path1 = scenario1.GenerateReferencePath();
        var path2 = scenario2.GenerateReferencePath();

        // Assert
        path1[^1].X.Should().BeApproximately(5.0, 0.01);
        path2[^1].X.Should().BeApproximately(20.0, 0.01);
    }
}