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 CircleScenarioTests
{
    [Fact]
    public void GenerateReferencePath_ShouldGenerateCircularPath()
    {
        // Arrange
        var scenario = TestHelpers.CreateCircleScenario(2.0);

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

        // Assert
        path.Should().NotBeNull();
        path.Count.Should().BeGreaterThan(0);
        
        // First and last points should be close (closed circle)
        var firstPoint = path[0];
        var lastPoint = path[^1];
        var distance = Math.Sqrt(Math.Pow(lastPoint.X - firstPoint.X, 2) + 
                                 Math.Pow(lastPoint.Y - firstPoint.Y, 2));
        distance.Should().BeLessThan(0.5); // Should be close to starting point
    }

    [Fact]
    public void GenerateReferencePath_ShouldHaveCorrectRadius()
    {
        // Arrange
        const double radius = 2.0;
        var scenario = TestHelpers.CreateCircleScenario(radius);

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

        // Assert
        // Check that points are approximately at the correct radius
        foreach (var point in path)
        {
            var distanceFromCenter = Math.Sqrt(point.X * point.X + point.Y * point.Y);
            distanceFromCenter.Should().BeApproximately(radius, 0.1);
        }
    }

    [Fact]
    public void IsGoalReached_WithPoseAtGoal_ShouldReturnTrue()
    {
        // Arrange
        var scenario = TestHelpers.CreateCircleScenario(2.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.CreateCircleScenario(2.0);
        var poseFarAway = new Pose2D(10.0, 10.0, 0.0);

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

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

    [Fact]
    public void GetGoalPose_ShouldReturnStartingPoint()
    {
        // Arrange
        var scenario = TestHelpers.CreateCircleScenario(2.0);

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

        // Assert
        // Goal should be at starting point (circle is closed)
        goalPose.X.Should().BeApproximately(2.0, 0.1);
        goalPose.Y.Should().BeApproximately(0.0, 0.1);
    }

    [Fact]
    public void GenerateReferencePath_WithDifferentRadii_ShouldGenerateCorrectPaths()
    {
        // Arrange
        var scenario1 = TestHelpers.CreateCircleScenario(1.0);
        var scenario2 = TestHelpers.CreateCircleScenario(5.0);

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

        // Assert
        // Check first point radius
        var radius1 = Math.Sqrt(path1[0].X * path1[0].X + path1[0].Y * path1[0].Y);
        var radius2 = Math.Sqrt(path2[0].X * path2[0].X + path2[0].Y * path2[0].Y);
        
        radius1.Should().BeApproximately(1.0, 0.1);
        radius2.Should().BeApproximately(5.0, 0.1);
    }
}