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

namespace RobotNet10.GlobalPathPlanner.ForkliftV2;

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

    private PathPlannerOptions Options = new()
    {
        LimitDistanceToEdge = 1,
        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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
            (var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(x, y, theta,
                                              new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y},
                                              Orientation.NONE,
                                              Options.LimitDistanceToEdge,
                                              Options.LimitDistanceToNode,
                                              cancellation.Token);

            if (path is null || path.Length < 1)
            {
                var ForkliftV1 = new ForkliftPathPlanner();
                ForkliftV1.SetData([.. Nodes], [.. Edges]);
                return ForkliftV1.PathPlanning(x,y,theta,goal.Id, cancellation.Token);
            }
            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)
    {
        throw new NotImplementedException();
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
            (var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(x, y, theta,
                                              new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                              goalDirection,
                                              Options.LimitDistanceToEdge,
                                              Options.LimitDistanceToNode,
                                              cancellation.Token);

            if (path is null || path.Length < 1)
            {
                var ForkliftV1 = new ForkliftPathPlanner();
                ForkliftV1.SetData([.. Nodes], [.. Edges]);
                return ForkliftV1.PathPlanningWithFinalDirection(x, y, theta, goal.Id, goalDirection, cancellation.Token);
            }
            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) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
            (var path, var closesEdge) = SSEAStarPlanner.PlanningWithGoalAngle(x, y, theta,
                                              new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                              goalAngle,
                                              Options.LimitDistanceToEdge,
                                              Options.LimitDistanceToNode,
                                              cancellation.Token);

            if (path is null || path.Length < 1)
            {
                var ForkliftV1 = new ForkliftPathPlanner();
                ForkliftV1.SetData([.. Nodes], [.. Edges]);
                return ForkliftV1.PathPlanningWithAngle(x, y, theta, goal.Id, goalAngle, cancellation.Token);
            }
            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) 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
            (var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(startNode, theta,
                                              new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                              Orientation.NONE,
                                              cancellation.Token);

            if (path is null || path.Length < 1)
            {
                var ForkliftV1 = new ForkliftPathPlanner();
                ForkliftV1.SetData([.. Nodes], [.. Edges]);
                return ForkliftV1.PathPlanningWithFinalDirection(startNodeId, theta, goal.Id, Orientation.NONE, cancellation.Token);
            }
            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)
    {
        throw new NotImplementedException();
    }

    public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
            (var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(startNode, theta,
                                              new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                              goalDirection,
                                              cancellation.Token);

            if (path is null || path.Length < 1)
            {
                var ForkliftV1 = new ForkliftPathPlanner();
                ForkliftV1.SetData([.. Nodes], [.. Edges]);
                return ForkliftV1.PathPlanningWithFinalDirection(startNodeId, theta, goal.Id, goalDirection, cancellation.Token);
            }
            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) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
            (var path, var closesEdge) = SSEAStarPlanner.PlanningWithGoalAngle(startNode, theta,
                                              new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
                                              goalAngle,
                                              cancellation.Token);

            if (path is null || path.Length < 1)
            {
                var ForkliftV1 = new ForkliftPathPlanner();
                ForkliftV1.SetData([.. Nodes], [.. Edges]);
                return ForkliftV1.PathPlanningWithAngle(startNodeId, theta, goal.Id, goalAngle, cancellation.Token);
            }
            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();
        }
    }
}