using RobotNet.VDA5050.Order;
using RobotNet10.Common;
using RobotNet10.Common.Models;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Services.Robot;
using RobotNet10.RobotApp.Services.Robot.Helper;
using RobotNet10.RobotApp.Services.Robot.Models;
using RobotNet10.RobotApp.Shared.Enums;

namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;

public class PurePursuit
{
    private double MaxAngularVelocity = 1.5;
    private double LookaheadDistance = 0.5;

    private readonly double ResolutionSplit = 0.1;

    private KDTree? KDTreeOrder;
    private KDTree? KDTreeWay;
    private OrderNode? LastNode;
    private Dictionary<string, (int start, int end)>? _segmentCache;

    public OrderNode? LastOrderNode = null;
    private int LastNavNodeIndex = 0;
    public int OnNodeIndex = 0;
    public NavigationNode? Goal;
    public List<NavigationNode> Waypoints_Value = [];

    public OrderNode[] OrderNodes = [];
    public OrderEdge[] OrderEdges = [];

    public PurePursuit WithLookheadDistance(double distance)
    {
        LookaheadDistance = distance;
        return this;
    }

    public PurePursuit WithMaxAngularVelocity(double vel)
    {
        MaxAngularVelocity = vel;
        return this;
    }

    public PurePursuit WithPath(Node[] nodes, Edge[] edges, double currentTheta)
    {
        if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
        if (edges.Length < 1) throw new SimulationException();
        if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
        (OrderNodes, OrderEdges) = OrderConverter.Validate(nodes, edges, currentTheta);
        Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
        BuildSegmentCache();
        KDTreeOrder = new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
        KDTreeWay = new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
        return this;
    }

    public void UpdateGoal(string goalId)
    {
        var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
        if (goal is not null) Goal = goal;
    }

    private NavigationNode[] PathSplit(OrderNode[] nodes, OrderEdge[] edges)
    {
        List<NavigationNode> navigationNode = [new()
            {
                Id = Guid.NewGuid(),
                NodeId = nodes[0].NodeId,
                X = nodes[0].X,
                Y = nodes[0].Y,
                Theta = nodes[0].Theta,
                Direction = edges[0].Direction,
            }];
        foreach (var edge in edges)
        {
            var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
            var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
            if (startNode is null) throw new PathPlannerException(RobotErrors.Error1008(edge.EdgeId, edge.StartNodeId));
            if (endNode is null) throw new PathPlannerException(RobotErrors.Error1009(edge.EdgeId, edge.EndNodeId));

            var spaceEdge = new SpaceEdge()
            {
                StartX = startNode.X,
                StartY = startNode.Y,
                EndX = endNode.X,
                EndY = endNode.Y,
                ControlPoint1X = edge.ControlPoint1X ?? 0,
                ControlPoint1Y = edge.ControlPoint1Y ?? 0,
                ControlPoint2X = edge.ControlPoint2X ?? 0,
                ControlPoint2Y = edge.ControlPoint2Y ?? 0,
                Degree = edge.Degree,
            };

            double length = SpaceCompute.GetEdgeLength(spaceEdge, ResolutionSplit);
            if (length <= 0) continue;
            double step = ResolutionSplit / length;

            for (double t = step; t <= 1 - step; t += step)
            {
                (double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
                navigationNode.Add(new()
                {
                    Id = Guid.NewGuid(),
                    NodeId = string.Empty,
                    X = x,
                    Y = y,
                    Theta = null,
                    Direction = edge.Direction,
                });
            }
            navigationNode.Add(new()
            {
                Id = Guid.NewGuid(),
                NodeId = endNode.NodeId,
                X = endNode.X,
                Y = endNode.Y,
                Theta = endNode.Theta,
                Direction = edge.Direction,
            });
        }
        return [.. navigationNode];
    }

    private void BuildSegmentCache()
    {
        _segmentCache = [];

        for (int i = 0; i < OrderNodes.Length - 1; i++)
        {
            var currentNodeId = OrderNodes[i].NodeId;

            // Xác định phạm vi: từ (i-1) đến (i+1)
            int rangeStart = Math.Max(0, i - 1);
            int rangeEnd = Math.Min(OrderNodes.Length - 1, i + 1);

            var startNodeId = OrderNodes[rangeStart].NodeId;
            var endNodeId = OrderNodes[rangeEnd].NodeId;

            // Tìm indices trong Waypoints_Value
            int waypointStartIdx = Waypoints_Value.FindIndex(n => n.NodeId == startNodeId);
            int waypointEndIdx = Waypoints_Value.FindIndex(n => n.NodeId == endNodeId);

            // Xử lý trường hợp không tìm thấy
            if (waypointStartIdx == -1) waypointStartIdx = 0;
            if (waypointEndIdx == -1) waypointEndIdx = Waypoints_Value.Count - 1;

            // Cache theo NodeId của OrderNode
            _segmentCache[currentNodeId] = (waypointStartIdx, waypointEndIdx);
        }
    }

    private (OrderNode node, int index)? GetOnNodeWithKDTree(double x, double y)
    {
        KDTreeOrder ??= new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
        LastNode ??= OrderNodes[0];
        var dx = LastNode.X - x;
        var dy = LastNode.Y - y;
        var minDistance = Math.Sqrt(dx * dx + dy * dy);
        var closesFindedNode = KDTreeOrder.FindNearest(x, y, minDistance);
        if (closesFindedNode == null) return null;

        var closesNodeIndex = Array.FindIndex(OrderNodes, n => n.NodeId == closesFindedNode?.Id);
        if (closesNodeIndex == -1) return null;

        if (OrderNodes[closesNodeIndex].NodeId != LastNode.NodeId)
        {
            var skipIndex = closesNodeIndex == 0 ? 0 : closesNodeIndex - 1;
            var newNodes = OrderNodes.Skip(skipIndex);
            KDTreeOrder = new KDTree(newNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));

            LastNode = OrderNodes[closesNodeIndex];
        }
        return (OrderNodes[closesNodeIndex], closesNodeIndex);
    }

    private (OrderNode node, int index) GetOnNode(double x, double y)
    {
        LastNode ??= OrderNodes[0];
        var lastIndex = Array.FindIndex(OrderNodes, n => n.NodeId == LastNode?.NodeId);
        lastIndex = Math.Max(0, lastIndex);

        var dx = LastNode.X - x;
        var dy = LastNode.Y - y;
        var minDistance = Math.Sqrt(dx * dx + dy * dy);

        OrderNode onNode = LastNode;
        int index = 0;
        for (int i = lastIndex; i < OrderNodes.Length; i++)
        {
            var node = OrderNodes[i]; // FIX: Dùng đúng array
            dx = x - node.X;
            dy = y - node.Y;
            var distance = dx * dx + dy * dy;
            if (distance < minDistance)
            {
                onNode = OrderNodes[i];
                minDistance = distance;
                index = i;
            }
        }
        return (onNode, index);
    }

    public (NavigationNode node, int index) GetOnNavNode(double x, double y)
    {
        double minDistance = double.MaxValue;
        NavigationNode onNode = Waypoints_Value[0];
        int index = 0;
        for (int i = 1; i < Waypoints_Value.Count; i++)
        {
            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
            if (distance < minDistance)
            {
                onNode = Waypoints_Value[i];
                minDistance = distance;
                index = i;
            }
        }
        return (onNode, index);
    }

    private (NavigationNode? node, int index) OnNodeLinear(double x, double y)
    {
        var (node, index) = GetOnNodeWithKDTree(x, y) ?? GetOnNode(x, y);

        int waypointStartIdx = 0;
        int waypointEndIdx = Waypoints_Value.Count - 1;
        if (_segmentCache is null)
        {
            var startIndex = Math.Max(0, index - 1);
            var endIndex = Math.Min(OrderNodes.Length - 1, index + 1);
            var navStartNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[startIndex].NodeId);
            waypointStartIdx = navStartNodeIndex == -1 ? 0 : navStartNodeIndex;
            var navEndNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[endIndex].NodeId);
            waypointEndIdx = navEndNodeIndex == -1 ? Waypoints_Value.Count - 1 : navEndNodeIndex;
        }
        else if (_segmentCache.TryGetValue(node.NodeId, out var range)) (waypointStartIdx, waypointEndIdx) = range;

        waypointStartIdx = Math.Min(waypointStartIdx, OnNodeIndex);

        var dx = OrderNodes[index].X - x;
        var dy = OrderNodes[index].Y - y;
        var minDistance = Math.Sqrt(dx * dx + dy * dy);

        NavigationNode? bestNode = null;
        int bestIndex = -1;

        for (int i = waypointStartIdx; i <= waypointEndIdx; i++)
        {
            var orderNodeId = Waypoints_Value[i].NodeId;

            // Tìm NavigationNode tương ứng
            var navIndex = Waypoints_Value.FindIndex(n => n.NodeId == orderNodeId);
            if (navIndex == -1) continue;

            var navNode = Waypoints_Value[navIndex];
            dx = x - navNode.X;
            dy = y - navNode.Y;
            var distanceSq = dx * dx + dy * dy;

            if (distanceSq < minDistance)
            {
                minDistance = distanceSq;
                bestNode = navNode;
                bestIndex = navIndex;
            }
        }

        return (bestNode, bestIndex);
    }

    private (NavigationNode? node, int index) OnNodeWithKDTree(double x, double y)
    {
        KDTreeWay ??= new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
        var kdtreeNode = KDTreeWay.FindNearest(x, y, double.MaxValue);
        if(kdtreeNode is not null && Guid.TryParse(kdtreeNode.Id, out Guid kdtreeNodeId))
        {
            var nodeIndex = Waypoints_Value.FindIndex(n => n.Id == kdtreeNodeId);
            if(nodeIndex != -1)  return (Waypoints_Value[nodeIndex], nodeIndex);
        }
        return (null, -1);
    }

    private (NavigationNode? node, int index) OnNode(double x, double y)
    {
        var (node, index) = OnNodeWithKDTree(x, y);
        if (node is null || index == -1) return OnNodeLinear(x, y);
        return (node, index);
    }

    private void UpdateLastOrderNode()
    {
        var oldNavNodes = Waypoints_Value.Skip(LastNavNodeIndex).Take(OnNodeIndex);
        var lastNavNode = oldNavNodes.LastOrDefault(n => string.IsNullOrEmpty(n.NodeId));
        if (lastNavNode is null) return;
        LastOrderNode = OrderNodes.FirstOrDefault(n => n.NodeId == lastNavNode.NodeId);
        if (LastOrderNode is not null) LastNavNodeIndex = Waypoints_Value.IndexOf(lastNavNode);
    }

    public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
    {
        if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
        NavigationNode? lookaheadStartPt = null;
        var (onNode, index) = OnNode(X_Ref, Y_Ref);
        if (onNode is null || Goal is null) return 0;
        OnNodeIndex = index;
        UpdateLastOrderNode();
        double lookDistance = 0;
        for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
        {
            lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
            if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
            {
                lookaheadStartPt = Waypoints_Value[i];
                break;
            }
        }
        lookaheadStartPt ??= Goal;
        if (onNode.Direction == RobotDirection.BACKWARD)
        {
            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
            else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
            Angle_Ref += Math.PI;
            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
        }
        var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;

        if (Math.Abs(distance) > Math.PI)
        {
            double minDistance;
            if (distance + Math.PI == 0.0) minDistance = 0.0;
            else
            {
                double data = (distance + Math.PI) / (2 * Math.PI);
                if (data < 0) data = Math.Round(data + 0.5);
                else data = Math.Round(data - 0.5);
                minDistance = distance + Math.PI - data * (2 * Math.PI);
                double checker = 0;
                if (minDistance != 0.0)
                {
                    checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
                }
                if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
                {
                    minDistance = 0.0;
                }
                else if (distance + Math.PI < 0.0)
                {
                    minDistance += Math.PI * 2;
                }
            }
            if (minDistance == 0.0 && distance + Math.PI > 0.0)
            {
                minDistance = Math.PI * 2;
            }
            distance = minDistance - Math.PI;
        }

        var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
        if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
        {
            if (AngularVelocity < 0.0)
            {
                AngularVelocity = -1.0;
            }
            else if (AngularVelocity > 0.0)
            {
                AngularVelocity = 1.0;
            }
            else if (AngularVelocity == 0.0)
            {
                AngularVelocity = 0.0;
            }
            AngularVelocity *= MaxAngularVelocity;
        }
        return AngularVelocity;
    }

}