BQP/srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/Differential/DifferentialPlanner.cs

using RobotNet10.GlobalPathPlanner.AStar;
using RobotNet10.GlobalPathPlanner.Model;

namespace RobotNet10.GlobalPathPlanner.Differential;

public class DifferentialPlanner : IPathPlanner
{
    private List<GlobalNode> Nodes = [];
    private List<GlobalEdge> Edges = [];
    private const double Ratio = 0.1;

    private PathPlannerOptions Options = new()
    {
        LimitDistanceToEdge = 2,
        LimitDistanceToNode = 0.3,
        ResolutionSplit = 0.1,
        ChangeOrientationAngle = 89
    };
    public void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
    {
        Nodes = [.. nodes];
        Edges = [.. edges];
    }

    public void SetOptions(PathPlannerOptions options)
    {
        Options = options;
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");

        using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
        if(Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);

        try
        {
            var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
            (var path, var closesEdge) = AStarPathPlanner.Planning(x, y,
                                                new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                                Options.LimitDistanceToEdge,
                                                Options.LimitDistanceToNode,
                                                cancellation.Token);

            if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}, {theta}]");
            if (path.Length == 1) return (path, []);

            var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);

            Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
            var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
            for (int i = 0; i < path.Length; i++)
            {
                path[i] = new GlobalNode
                {
                    Id = path[i].Id,
                    X = path[i].X,
                    Y = path[i].Y,
                    Name = path[i].Name,
                    MapId = path[i].MapId,
                    Orientation = FinalDirection[i]
                };
            }

            return ([.. path], [.. edgeplannings]);
        }
        catch (OperationCanceledException)
        {
            if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
            else throw new TimeoutException();
        }
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
        if (basicPath.Nodes.Length < 1) return basicPath;

        if (startDiretion == basicPath.Nodes[0].Orientation || startDiretion == Orientation.NONE) return basicPath;
        foreach (var node in basicPath.Nodes)
        {
            node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
        }

        return basicPath;
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
        if (basicPath.Nodes.Length < 1) return basicPath;

        if (goalDirection == basicPath.Nodes[^1].Orientation || goalDirection == Orientation.NONE) return basicPath;
        foreach (var node in basicPath.Nodes)
        {
            node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
        }

        return basicPath;
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
        if (basicPath.Nodes.Length < 1) return basicPath;

        Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
        if (goalDirection == basicPath.Nodes[^1].Orientation) return basicPath;
        foreach (var node in basicPath.Nodes)
        {
            node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
        }

        return basicPath;
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
    {
        var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");

        using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
        if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);

        try
        {
            var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
            (var path, var closesEdge) = AStarPathPlanner.Planning(startNode, goal, cancellation.Token);

            if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
            if (path.Length == 1) return (path, []);

            var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);

            Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
            var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
            for (int i = 0; i < path.Length; i++)
            {
                path[i] = new GlobalNode
                {
                    Id = path[i].Id,
                    X = path[i].X,
                    Y = path[i].Y,
                    Name = path[i].Name,
                    MapId = path[i].MapId,
                    Orientation = FinalDirection[i]
                };
            }

            return ([.. path], [.. edgeplannings]);
        }
        catch(OperationCanceledException)
        {
            if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
            else throw new TimeoutException();
        }
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
        if (basicPath.Nodes.Length < 1) return basicPath;

        if (startDiretion == basicPath.Nodes[0].Orientation || startDiretion == Orientation.NONE) return basicPath;
        foreach (var node in basicPath.Nodes)
        {
            node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
        }

        return basicPath;
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
        if (basicPath.Nodes.Length < 1) return basicPath;

        if (goalDirection == basicPath.Nodes[^1].Orientation || goalDirection == Orientation.NONE) return basicPath;
        foreach (var node in basicPath.Nodes)
        {
            node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
        }

        return basicPath;
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
    {
        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
        if (basicPath.Nodes.Length < 1) return basicPath;

        Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
        if (goalDirection == basicPath.Nodes[^1].Orientation) return basicPath;
        foreach (var node in basicPath.Nodes)
        {
            node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
        }

        return basicPath;
    }
}