using RobotNet10.GlobalPathPlanner.Model;
using RobotNet10.GlobalPathPlanner.Space;

namespace RobotNet10.GlobalPathPlanner.ForkliftV2;

public class SSEAStarPlanner(List<GlobalNode> Nodes, List<GlobalEdge> Edges, PathPlannerOptions Options)
{
    private const double Ratio = 0.01;
    private GlobalNode? GetOnNode(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
    {
        if (cancellationToken?.IsCancellationRequested == true) return null;
        KDTree KDTree = new(Nodes);
        return KDTree.FindNearest(x, y, limitDistance);
    }

    private GlobalEdge[]? GetClosesEdges(GlobalNode nodeRef, double limitDistance)
    {
        double minDistance = double.MaxValue;
        List<GlobalEdge> edgesResult = [];
        foreach (var edge in Edges)
        {
            var startNode = Nodes.FirstOrDefault(node => node.Id == edge.StartNodeId);
            var endNode = Nodes.FirstOrDefault(node => node.Id == edge.EndNodeId);
            if (startNode is null || endNode is null) continue;

            var distance = MathExtensions.DistanceToEdge(nodeRef, startNode, endNode, edge);
            if (distance < minDistance)
            {
                minDistance = distance;
                edgesResult = [edge];
                var reverseEdge = Edges.FirstOrDefault(e => e.EndNodeId == startNode.Id && e.StartNodeId == endNode.Id);
                if (reverseEdge != null) edgesResult = [.. edgesResult, reverseEdge];
            }
        }
        if (minDistance <= limitDistance) return [.. edgesResult];
        else return null;
    }

    public List<GlobalNode> GetNegativeNodes(Guid nodeId)
    {
        var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
        if (node is null) return [];

        var listNodesNegative = new List<GlobalNode>();
        var listPaths = Edges.Where(p =>  p.StartNodeId == nodeId);
        foreach (var path in listPaths)
        {
            var negativeNode = Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
            if (negativeNode != null) listNodesNegative.Add(negativeNode);
        }
        return listNodesNegative;
    }

    private double GetNegativeCost(SSEAStarNode currenNode, SSEAStarNode negativeNode)
    {
        var negativeEdges = Edges.Where(e => e.StartNodeId == currenNode.Id && e.EndNodeId == negativeNode.Id || e.StartNodeId == negativeNode.Id && e.EndNodeId == currenNode.Id).ToList();
        double minDistance = double.MaxValue;
        foreach (var edge in negativeEdges)
        {
            var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
            var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
            if (startNode is null || endNode is null) return 0;
            var distance = MathExtensions.GetEdgeLength(startNode, endNode, edge);
            if (distance < minDistance) minDistance = distance;
        }
        return minDistance != double.MaxValue ? minDistance : 0;
    }

    private List<SSEAStarNode> GetNegativeAStarNodes(SSEAStarNode nodeCurrent, GlobalNode endNode)
    {
        var possiblePointNegative = new List<SSEAStarNode>();
        if (nodeCurrent.Id == endNode.Id) return possiblePointNegative;

        var listNodesNegative = GetNegativeNodes(nodeCurrent.Id);

        foreach (var negativeNode in listNodesNegative)
        {
            if (nodeCurrent.Parent is null) continue;
            var nodeDtoCurrent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Id);
            var nodeDtoNegative = Nodes.FirstOrDefault(n => n.Id == negativeNode.Id);
            var nodeDtoParent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Parent.Id);
            var negativeEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == negativeNode.Id);
            var parentEdge = Edges.FirstOrDefault(e => e.EndNodeId == nodeCurrent.Id && e.StartNodeId == nodeCurrent.Parent.Id);

            if (nodeDtoCurrent is null || nodeDtoNegative is null || negativeEdge is null) continue;

            var nearNodeNevgative = MathExtensions.BezierPoint(Ratio, nodeDtoCurrent, nodeDtoNegative, negativeEdge);
            var nearNodeParent = nodeDtoParent is not null && parentEdge is not null ? MathExtensions.BezierPoint(Ratio, nodeDtoParent, nodeDtoCurrent, parentEdge) :
                                 new()
                                 {
                                     Id = nodeCurrent.Parent.Id,
                                     X = nodeCurrent.Parent.X,
                                     Y = nodeCurrent.Parent.Y,
                                     Name = nodeCurrent.Parent.Name
                                 };

            var angle = MathExtensions.GetAngle(nodeDtoCurrent, nearNodeNevgative, nearNodeParent);
            Orientation orientation = angle < Options.ChangeOrientationAngle ? nodeCurrent.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD : nodeCurrent.Orientation;

            var nodeNegative = new SSEAStarNode
            {
                Id = negativeNode.Id,
                X = negativeNode.X,
                Y = negativeNode.Y,
                Name = negativeNode.Name,
                Orientation = orientation,
                Parent = nodeCurrent
            };

            var cost = GetNegativeCost(nodeCurrent, nodeNegative);
            cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(nodeCurrent.X - nodeNegative.X, 2) + Math.Pow(nodeCurrent.Y - nodeNegative.Y, 2));
            nodeNegative.Cost = cost + nodeCurrent.Cost + (orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0);
            var distance = Math.Abs(endNode.X - nodeNegative.X) + Math.Abs(endNode.Y - nodeNegative.Y);
            nodeNegative.Heuristic = distance * (1 + (orientation == Orientation.BACKWARD ? Math.Sqrt(2) / 2 : 0.0));
            possiblePointNegative.Add(nodeNegative);
        }
        if (nodeCurrent.NegativeNodes is not null && nodeCurrent.NegativeNodes.Count > 0) possiblePointNegative.AddRange(nodeCurrent.NegativeNodes);
        return possiblePointNegative;
    }

    public List<SSEAStarNode> Find(SSEAStarNode startNode, GlobalNode goal, Orientation goalDirection, CancellationToken? cancellationToken = null)
    {
        try
        {
            var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
            var visitedNodes = new HashSet<SSEAStarNode>();
            var path = new List<SSEAStarNode>();
            var shortestPath = new HashSet<SSEAStarNode>();

            activeNodes.Enqueue(startNode);

            while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
            {
                var checkNode = activeNodes.Dequeue();
                if (checkNode.Id == goal.Id)
                {
                    if (checkNode.Orientation == goalDirection || goalDirection == Orientation.NONE)
                    {
                        var node = checkNode;
                        while (node != null)
                        {
                            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
                            path.Add(node);
                            node = node.Parent;
                        }
                        return path;
                    }
                    else
                    {
                        var node = checkNode;
                        while (node != null)
                        {
                            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
                            shortestPath.Add(node);
                            node = node.Parent;
                        }
                    }
                }

                visitedNodes.Add(checkNode);

                var listNodeNegative = GetNegativeAStarNodes(checkNode, goal);
                foreach (var node in listNodeNegative)
                {
                    if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
                    {
                        if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
                        {
                            visitedNodes.Remove(value);
                            activeNodes.Enqueue(node);
                        }
                        continue;
                    }

                    var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Orientation == node.Orientation);
                    if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
                    {
                        activeNodes.Items.Remove(activeNode);
                        activeNodes.Enqueue(node);
                    }
                    else if (activeNode is null)
                    {
                        activeNodes.Enqueue(node);
                    }
                }
            }

            return [];
        }
        catch (OperationCanceledException)
        {
            return [];
        }
    }

    public List<SSEAStarNode> Find(SSEAStarNode startNode, GlobalNode goal, double goalAngle, CancellationToken? cancellationToken = null)
    {
        try
        {
            var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
            var visitedNodes = new HashSet<SSEAStarNode>();
            var path = new List<SSEAStarNode>();
            var shortestPath = new HashSet<SSEAStarNode>();

            activeNodes.Enqueue(startNode);

            while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
            {
                var checkNode = activeNodes.Dequeue();
                if (checkNode.Id == goal.Id)
                {
                    if (checkNode.Parent is not null)
                    {
                        var nodeParentDto = Nodes.FirstOrDefault(n => n.Id == checkNode.Parent.Id);
                        var edge = Edges.FirstOrDefault(e => e.EndNodeId == checkNode.Id && e.StartNodeId == checkNode.Parent.Id);
                        if (edge is not null && nodeParentDto is not null)
                        {
                            var nearParent = MathExtensions.BezierPoint(Ratio, nodeParentDto, goal, edge);
                            var nearGoalNode = new GlobalNode()
                            {
                                X = goal.X + Math.Cos(goalAngle * Math.PI / 180),
                                Y = goal.Y + Math.Sin(goalAngle * Math.PI / 180),
                            };

                            var angle = MathExtensions.GetAngle(goal, nearParent, nearGoalNode);
                            Orientation goalDirection = angle < Options.ChangeOrientationAngle ? Orientation.BACKWARD : Orientation.FORWARD;
                            if (checkNode.Orientation == goalDirection)
                            {
                                var returnNode = checkNode;
                                while (returnNode != null)
                                {
                                    if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
                                    path.Add(returnNode);
                                    returnNode = returnNode.Parent;
                                }
                                return path;
                            }
                        }
                    }

                    var node = checkNode;
                    while (node != null)
                    {
                        if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
                        shortestPath.Add(node);
                        node = node.Parent;
                    }
                }

                visitedNodes.Add(checkNode);

                var listNodeNegative = GetNegativeAStarNodes(checkNode, goal);
                foreach (var node in listNodeNegative)
                {
                    if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
                    {
                        if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
                        {
                            visitedNodes.Remove(value);
                            activeNodes.Enqueue(node);
                        }
                        continue;
                    }

                    var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Orientation == node.Orientation);
                    if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
                    {
                        activeNodes.Items.Remove(activeNode);
                        activeNodes.Enqueue(node);
                    }
                    else if (activeNode is null)
                    {
                        activeNodes.Enqueue(node);
                    }
                }
            }

            return [];
        }
        catch (OperationCanceledException)
        {
            return [];
        }
    }

    private SSEAStarNode GetClosesNode(GlobalNode closesNode, GlobalNode goal, double theta)
    {
        SSEAStarNode closesAStarNode = new()
        {
            Id = closesNode.Id,
            X = closesNode.X,
            Y = closesNode.Y,
            Name = closesNode.Name,
        };
        foreach (var negativeNode in GetNegativeNodes(closesAStarNode.Id))
        {
            SSEAStarNode closesAStarNodeParent = new()
            {
                Id = closesNode.Id,
                X = closesNode.X,
                Y = closesNode.Y,
                Name = closesNode.Name,
            };
            var RobotNearNode = new GlobalNode()
            {
                X = closesAStarNode.X + Math.Cos(theta * Math.PI / 180),
                Y = closesAStarNode.Y + Math.Sin(theta * Math.PI / 180),
            };

            var angle = MathExtensions.GetAngle(closesNode, negativeNode, RobotNearNode);
            Orientation orientation = angle < 91 ? Orientation.FORWARD : Orientation.BACKWARD;

            var cost = GetNegativeCost(closesAStarNode, new() { Id = negativeNode.Id, X = negativeNode.X, Y = negativeNode.Y });
            cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(closesAStarNode.X - negativeNode.X, 2) + Math.Pow(closesAStarNode.Y - negativeNode.Y, 2));
            cost += orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
            closesAStarNodeParent.Orientation = orientation;
            closesAStarNode.NegativeNodes.Add(new()
            {
                Id = negativeNode.Id,
                X = negativeNode.X,
                Y = negativeNode.Y,
                Name = negativeNode.Name,
                Orientation = orientation,
                Cost = cost,
                Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
                Parent = closesAStarNodeParent,
            });
        }
        return closesAStarNode;
    }

    private SSEAStarNode[] GetStartNegativeNodes(GlobalEdge[] closesEdges, GlobalNode goal, SSEAStarNode robotNode, double theta)
    {
        List<SSEAStarNode> negativeNodes = [];
        foreach(var edge in closesEdges)
        {
            var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
            if (endNode == null) continue;
            SSEAStarNode closesAStarNodeParent = new()
            {
                Id = robotNode.Id,
                X = robotNode.X,
                Y = robotNode.Y,
                Name = robotNode.Name,
            };
            var RobotNearNode = new GlobalNode()
            {
                X = robotNode.X + Math.Cos(theta * Math.PI / 180),
                Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
            };
            var angle = MathExtensions.GetAngle(new() { X = robotNode.X, Y = robotNode.Y}, endNode, RobotNearNode);
            Orientation orientation = angle < Options.ChangeOrientationAngle ? Orientation.FORWARD : Orientation.BACKWARD;

            double cost = Math.Sqrt(Math.Pow(robotNode.X - endNode.X, 2) + Math.Pow(robotNode.Y - endNode.Y, 2));
            cost += orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
            closesAStarNodeParent.Orientation = orientation;
            negativeNodes.Add(new()
            {
                Id = endNode.Id,
                X = endNode.X,
                Y = endNode.Y,
                Name = endNode.Name,
                Orientation = orientation,
                Cost = cost,
                Heuristic = Math.Abs(goal.X - endNode.X) + Math.Abs(goal.Y - endNode.Y),
                Parent = closesAStarNodeParent,
            });
        }
        return [.. negativeNodes];
    }

    public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithFinalDirection(double x, double y, double theta, GlobalNode goal, Orientation goalDirection, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
    {
        var Path = new List<GlobalNode>();

        SSEAStarNode RobotNode = new()
        {
            Id = Guid.NewGuid(),
            X = x,
            Y = y,
            Name = "RobotCurrentNode",
        };
        var closesNode = GetOnNode(x, y, maxDistanceToNode);
        if (closesNode is not null)
        {
            if (closesNode.Id == goal.Id) return ([goal], null);
            RobotNode = GetClosesNode(closesNode, goal, theta);
        }
        else
        {
            var closesEdges = GetClosesEdges(new() { X = x, Y = y }, maxDistanceToEdge);
            if (closesEdges is null || closesEdges.Length == 0) throw new Exception("The robot is too far from the route");

            var edgeToGoal = closesEdges.FirstOrDefault(e => e.EndNodeId == goal.Id);
            if (edgeToGoal != null)
            {
                var endNode = Nodes.FirstOrDefault(n => n.Id == edgeToGoal.EndNodeId);
                if (endNode != null) return ([new() {
                                                        Id = RobotNode.Id,
                                                        X = RobotNode.X,
                                                        Y = RobotNode.Y,
                                                        Name = RobotNode.Name,
                                                        MapId = endNode.MapId,
                                                    }, endNode], edgeToGoal);
            }

            RobotNode.NegativeNodes.AddRange(GetStartNegativeNodes(closesEdges, goal, RobotNode, theta));
        }

        if (RobotNode.NegativeNodes.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");

        var path = Find(RobotNode, goal, goalDirection, cancellationToken);
        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
        if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");
        path.Reverse();
        foreach (var node in path)
        {
            if (node.Id == path.First().Id)
            {
                Path.Add(new()
                {
                    Id = node.Id,
                    Name = node.Name,
                    X = node.X,
                    Y = node.Y,
                    Orientation = node.Orientation,
                });
                continue;
            }
            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
            Path.Add(new GlobalNode
            {
                Id = nodedb.Id,
                X = nodedb.X,
                Y = nodedb.Y,
                Name = nodedb.Name,
                MapId = nodedb.MapId,
                Orientation = node.Orientation
            });
        }
        var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
        return ([.. Path], startEdge);
    }

    public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithGoalAngle(double x, double y, double theta, GlobalNode goal, double goalAngle, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
    {
        var Path = new List<GlobalNode>();

        SSEAStarNode RobotNode = new()
        {
            Id = Guid.NewGuid(),
            X = x,
            Y = y,
            Name = "RobotCurrentNode",
        };
        var closesNode = GetOnNode(x, y, maxDistanceToNode);
        if (closesNode is not null)
        {
            if (closesNode.Id == goal.Id) return ([goal], null);
            RobotNode = GetClosesNode(closesNode, goal, theta);
        }
        else
        {
            var closesEdges = GetClosesEdges(new() { X = x, Y = y }, maxDistanceToEdge);
            if (closesEdges is null || closesEdges.Length == 0) throw new Exception("The robot is too far from the route");

            var edgeToGoal = closesEdges.FirstOrDefault(e => e.EndNodeId == goal.Id);
            if (edgeToGoal != null)
            {
                var endNode = Nodes.FirstOrDefault(n => n.Id == edgeToGoal.EndNodeId);
                if (endNode != null) return ([new() {
                                                        Id = RobotNode.Id,
                                                        X = RobotNode.X,
                                                        Y = RobotNode.Y,
                                                        Name = RobotNode.Name,
                                                        MapId = endNode.MapId,
                                                    }, endNode], edgeToGoal);
            }

            RobotNode.NegativeNodes.AddRange(GetStartNegativeNodes(closesEdges, goal, RobotNode, theta));
        }

        if (RobotNode.NegativeNodes.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");

        var path = Find(RobotNode, goal, goalAngle, cancellationToken);
        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
        if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");
        path.Reverse();
        foreach (var node in path)
        {
            if (node.Id == path.First().Id)
            {
                Path.Add(new()
                {
                    Id = node.Id,
                    Name = node.Name,
                    X = node.X,
                    Y = node.Y,
                    Orientation = node.Orientation,
                });
                continue;
            }
            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
            Path.Add(new GlobalNode
            {
                Id = nodedb.Id,
                X = nodedb.X,
                Y = nodedb.Y,
                Name = nodedb.Name,
                MapId = nodedb.MapId,
                Orientation = node.Orientation
            });
        }
        var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
        return ([.. Path], startEdge);
    }

    public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithFinalDirection(GlobalNode startNode, double theta, GlobalNode goal, Orientation goalOrientation, CancellationToken? cancellationToken = null)
    {
        var Path = new List<GlobalNode>();
        SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
        var path = Find(RobotNode, goal, goalOrientation, cancellationToken);
        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
        if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
        path.Reverse();
        foreach (var node in path)
        {
            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
            Path.Add(new GlobalNode
            {
                Id = nodedb.Id,
                X = nodedb.X,
                Y = nodedb.Y,
                Name = nodedb.Name,
                MapId = nodedb.MapId,
                Orientation = node.Orientation
            });
        }
        var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
        return ([.. Path], startEdge);
    }

    public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithGoalAngle(GlobalNode startNode, double theta, GlobalNode goal, double goalAngle, CancellationToken? cancellationToken = null)
    {
        var Path = new List<GlobalNode>();
        SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
        var path = Find(RobotNode, goal, goalAngle, cancellationToken);
        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
        if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
        path.Reverse();
        foreach (var node in path)
        {
            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
            Path.Add(new GlobalNode
            {
                Id = nodedb.Id,
                X = nodedb.X,
                Y = nodedb.Y,
                Name = nodedb.Name,
                MapId = nodedb.MapId,
                Orientation = node.Orientation
            });
        }
        var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
        return ([.. Path], startEdge);
    }
}