BQP/srcs/RobotNet10/Tests/RobotNet10.GlobalPathPlanner.Test/DifferentialPlannerTests.cs

using RobotNet10.GlobalPathPlanner.Differential;
using RobotNet10.GlobalPathPlanner.Model;

namespace RobotNet10.GlobalPathPlanner.UnitTest;

public class DifferentialPlannerTests
{
    private readonly DifferentialPlanner _planner;
    private readonly (GlobalNode[] Nodes, GlobalEdge[] Edges) _simpleGraph;

    public DifferentialPlannerTests()
    {
        _planner = new DifferentialPlanner();
        _simpleGraph = TestHelpers.CreateSimpleLinearGraph();
    }

    [Fact]
    public void SetData_ShouldStoreNodesAndEdges()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;

        // Act
        _planner.SetData(nodes, edges);

        // Assert - No exception should be thrown
        Assert.True(true);
    }

    [Fact]
    public void SetData_WithEmptyArrays_ShouldNotThrow()
    {
        // Arrange
        var nodes = Array.Empty<GlobalNode>();
        var edges = Array.Empty<GlobalEdge>();

        // Act & Assert
        _planner.SetData(nodes, edges);
        Assert.True(true);
    }

    [Fact]
    public void SetOptions_ShouldUpdateOptions()
    {
        // Arrange
        var options = new PathPlannerOptions
        {
            LimitDistanceToEdge = 2.0,
            LimitDistanceToNode = 0.5,
            ResolutionSplit = 0.2,
            TimeOut = TimeSpan.FromSeconds(5),
            ChangeOrientationAngle = 90.0
        };

        // Act
        _planner.SetOptions(options);

        // Assert - No exception should be thrown
        Assert.True(true);
    }

    [Fact]
    public void PathPlanning_WithValidGraph_ShouldReturnPath()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var startX = 0.0;
        var startY = 0.0;
        var startTheta = 0.0;
        var goalId = nodes[2].Id; // Node3

        // Act
        var (pathNodes, pathEdges) = _planner.PathPlanning(startX, startY, startTheta, goalId);

        // Assert
        Assert.NotNull(pathNodes);
        Assert.NotNull(pathEdges);
        Assert.NotEmpty(pathNodes);
        Assert.True(pathNodes.Length >= 2); // At least start and goal
        Assert.Equal(goalId, pathNodes[^1].Id); // Last node should be goal
    }

    [Fact]
    public void PathPlanning_WithInvalidGoalId_ShouldThrowException()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var invalidGoalId = Guid.NewGuid();

        // Act & Assert
        var exception = Assert.Throws<Exception>(() =>
            _planner.PathPlanning(0.0, 0.0, 0.0, invalidGoalId));

        Assert.Contains("does not exist", exception.Message);
    }

    [Fact]
    public void PathPlanning_WithDisconnectedGraph_ShouldThrowException()
    {
        // Arrange
        var (nodes, edges) = TestHelpers.CreateDisconnectedGraph();
        _planner.SetData(nodes, edges);

        var startX = 0.0;
        var startY = 0.0;
        var startTheta = 0.0;
        var goalId = nodes[2].Id; // Node3 is disconnected

        // Act & Assert
        var exception = Assert.Throws<Exception>(() =>
            _planner.PathPlanning(startX, startY, startTheta, goalId));

        Assert.Contains("does not exist", exception.Message);
    }

    [Fact]
    public void PathPlanning_FromNodeId_ShouldReturnPath()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var startNodeId = nodes[0].Id; // Node1
        var startTheta = 0.0;
        var goalId = nodes[2].Id; // Node3

        // Act
        var (pathNodes, pathEdges) = _planner.PathPlanning(startNodeId, startTheta, goalId);

        // Assert
        Assert.NotNull(pathNodes);
        Assert.NotNull(pathEdges);
        Assert.NotEmpty(pathNodes);
        Assert.Equal(startNodeId, pathNodes[0].Id); // First node should be start
        Assert.Equal(goalId, pathNodes[^1].Id); // Last node should be goal
    }

    [Fact]
    public void PathPlanning_FromInvalidNodeId_ShouldThrowException()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var invalidStartNodeId = Guid.NewGuid();
        var goalId = nodes[2].Id;

        // Act & Assert
        var exception = Assert.Throws<Exception>(() =>
            _planner.PathPlanning(invalidStartNodeId, 0.0, goalId));

        Assert.Contains("does not exist", exception.Message);
    }

    [Fact]
    public void PathPlanningWithStartDirection_WithForwardDirection_ShouldReturnPath()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var startX = 0.0;
        var startY = 0.0;
        var startTheta = 0.0;
        var goalId = nodes[2].Id;

        // Act
        var (pathNodes, pathEdges) = _planner.PathPlanningWithStartDirection(
            startX, startY, startTheta, goalId, Orientation.FORWARD);

        // Assert
        Assert.NotNull(pathNodes);
        Assert.NotEmpty(pathNodes);
    }

    [Fact]
    public void PathPlanningWithStartDirection_WithBackwardDirection_ShouldReturnPath()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var startX = 0.0;
        var startY = 0.0;
        var startTheta = 0.0;
        var goalId = nodes[2].Id;

        // Act
        var (pathNodes, pathEdges) = _planner.PathPlanningWithStartDirection(
            startX, startY, startTheta, goalId, Orientation.BACKWARD);

        // Assert
        Assert.NotNull(pathNodes);
        Assert.NotEmpty(pathNodes);
    }

    [Fact]
    public void PathPlanningWithFinalDirection_WithForwardDirection_ShouldReturnPath()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var startX = 0.0;
        var startY = 0.0;
        var startTheta = 0.0;
        var goalId = nodes[2].Id;

        // Act
        var (pathNodes, pathEdges) = _planner.PathPlanningWithFinalDirection(
            startX, startY, startTheta, goalId, Orientation.FORWARD);

        // Assert
        Assert.NotNull(pathNodes);
        Assert.NotEmpty(pathNodes);
    }

    [Fact]
    public void PathPlanningWithAngle_WithValidAngle_ShouldReturnPath()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var startX = 0.0;
        var startY = 0.0;
        var startTheta = 0.0;
        var goalId = nodes[2].Id;
        var goalAngle = 90.0;

        // Act
        var (pathNodes, pathEdges) = _planner.PathPlanningWithAngle(
            startX, startY, startTheta, goalId, goalAngle);

        // Assert
        Assert.NotNull(pathNodes);
        Assert.NotEmpty(pathNodes);
    }

    [Fact]
    public void PathPlanning_WithTimeout_ShouldThrowTimeoutException()
    {
        // Arrange
        var (nodes, edges) = TestHelpers.CreateSquareGraph();
        _planner.SetData(nodes, edges);

        var options = new PathPlannerOptions
        {
            LimitDistanceToEdge = 1.0,
            LimitDistanceToNode = 0.3,
            ResolutionSplit = 0.1,
            TimeOut = TimeSpan.FromMilliseconds(1), // Very short timeout
            ChangeOrientationAngle = 89.0
        };
        _planner.SetOptions(options);

        var startX = 0.0;
        var startY = 0.0;
        var startTheta = 0.0;
        var goalId = nodes[3].Id;

        // Act & Assert
        // Note: This test might not always timeout depending on system performance
        // It's included as an example of timeout handling
        try
        {
            _planner.PathPlanning(startX, startY, startTheta, goalId);
        }
        catch (TimeoutException)
        {
            // Expected behavior
            Assert.True(true);
        }
        catch (Exception)
        {
            // May not timeout on fast systems
            Assert.True(true);
        }
    }

    [Fact]
    public void PathPlanning_WithCancellation_ShouldThrowOperationCanceledException()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        using var cts = new CancellationTokenSource();
        cts.Cancel(); // Cancel immediately

        // Act & Assert
        Assert.Throws<OperationCanceledException>(() =>
            _planner.PathPlanning(0.0, 0.0, 0.0, nodes[2].Id, cts.Token));
    }

    [Fact]
    public void PathPlanning_WithSameStartAndGoal_ShouldReturnSingleNode()
    {
        // Arrange
        var (nodes, edges) = _simpleGraph;
        _planner.SetData(nodes, edges);

        var startNodeId = nodes[0].Id;
        var goalId = nodes[0].Id; // Same as start

        // Act
        var (pathNodes, pathEdges) = _planner.PathPlanning(startNodeId, 0.0, goalId);

        // Assert
        Assert.NotNull(pathNodes);
        Assert.Single(pathNodes);
        Assert.Empty(pathEdges);
        Assert.Equal(goalId, pathNodes[0].Id);
    }
}