using System;
using UnityEngine;

namespace Game.Plugins.JNGame.Sync.Frame.AstarPath.RVO
{
    /// <summary>  
    /// 用于快速寻找RVO代理的最近邻居的四叉树。  
    /// 详见：Pathfinding.RVO.Simulator  
    /// </summary>
    public class JNRVOQuadtree
    {
        
        const int LeafSize = 15;
        float maxRadius = 0;
        
        /// <summary>  
        /// 用于存储RVO代理的四叉树的节点。  
        /// 详见：Pathfinding.GraphNode，该节点类用于存储路径查找数据。  
        /// </summary>  
        struct Node {  
            
            public int child00; // 可能是一个指向子节点的引用或索引。  
            public JNRVOAgent linkedList; // 代理链表的头节点。  
            public byte count; // 该节点中的代理数量。  
            public float maxSpeed; // 该节点中的代理的最大速度。  
            
            public void Add(JNRVOAgent agent) { // 添加新的代理到链表的尾部的方法。  
                agent.next = linkedList; // 将新代理的next指向当前的链表头。  
                linkedList = agent; // 将链表头更新为新代理。  
            }  
            
            /// <summary>  
            /// 将代理分配到子节点中。  
            /// 用于将一个节点中的代理分发到其子节点中。通常在节点被分割后使用。  
            /// </summary>  
            public void Distribute(Node[] nodes, Rect r) {  
                Vector2 c = r.center;  // 获取矩形的中心点  
  
                while (linkedList != null) {  // 当代理链表不为空时  
                    JNRVOAgent nx = linkedList.next;  // 保存下一个代理的引用  
                    var index = child00 + (linkedList.position.x > c.x ? 2 : 0) + (linkedList.position.y > c.y ? 1 : 0);  // 计算代理应该分配到的子节点的索引  
                    nodes[index].Add(linkedList);  // 将代理添加到对应的子节点中  
                    linkedList = nx;  // 移动到链表中的下一个代理  
                }  
                count = 0;  // 重置代理计数器  
            }  
  
            /// <summary>  
            /// 计算节点的最大速度。  
            /// 用于计算一个节点及其所有子节点的最大速度。  
            /// </summary>  
            public float CalculateMaxSpeed(Node[] nodes, int index) {  
                if (child00 == index) {  // 如果当前节点是叶节点  
                    // 遍历叶节点中的所有代理，找到最大的速度  
                    for (var agent = linkedList; agent != null; agent = agent.next) {  
                        maxSpeed = Math.Max(maxSpeed, agent.CalculatedSpeed);  
                    }  
                } else {  // 如果当前节点不是叶节点  
                    maxSpeed = Math.Max(nodes[child00].CalculateMaxSpeed(nodes, child00), nodes[child00+1].CalculateMaxSpeed(nodes, child00+1));  // 分别计算两个子节点的最大速度，取两者中的最大值  
                    maxSpeed = Math.Max(maxSpeed, nodes[child00+2].CalculateMaxSpeed(nodes, child00+2));  // 计算第三个子节点的最大速度，并与之前的最大值比较并更新  
                    maxSpeed = Math.Max(maxSpeed, nodes[child00+3].CalculateMaxSpeed(nodes, child00+3));  // 计算第四个子节点的最大速度，并与之前的最大值比较并更新  
                }  
                return maxSpeed;  // 返回计算得到的最大速度  
            }
        }
        
        Node[] nodes = new Node[16];  
        int filledNodes = 1;  
  
        Rect bounds;  
  
        /// <summary>从树中移除所有代理</summary>  
        public void Clear() {  
            nodes[0] = new Node();  
            filledNodes = 1;  
            maxRadius = 0;  
        }  
  
        public void SetBounds(Rect r) {  
            bounds = r;  
        }  
  
        int GetNodeIndex() {  
            if (filledNodes + 4 >= nodes.Length) {  
                var nds = new Node[nodes.Length * 2];  
                for (int i = 0; i < nodes.Length; i++) nds[i] = nodes[i];  
                nodes = nds;  
            }  
            nodes[filledNodes] = new Node();  
            nodes[filledNodes].child00 = filledNodes;  
            filledNodes++;  
            nodes[filledNodes] = new Node();  
            nodes[filledNodes].child00 = filledNodes;  
            filledNodes++;  
            nodes[filledNodes] = new Node();  
            nodes[filledNodes].child00 = filledNodes;  
            filledNodes++;  
            nodes[filledNodes] = new Node();  
            nodes[filledNodes].child00 = filledNodes;  
            filledNodes++;  
            return filledNodes - 4;  
        }
        
        /// <summary>  
        /// 将一个代理添加到树中。  
        /// 注意：代理不能被多次添加到同一棵树中。  
        /// </summary>  
        public void Insert(JNRVOAgent agent) {  
            int i = 0;  
            Rect r = bounds;  
            Vector2 p = new Vector2(agent.position.x, agent.position.y);  
  
            agent.next = null;  
  
            maxRadius = System.Math.Max(agent.radius, maxRadius);  
  
            int depth = 0;  
  
            while (true) {  
                depth++;  
  
                if (nodes[i].child00 == i) {  
                    // 叶节点。如果代理数量大于等于LeafSize，则深度超过10时停止（避免过多的代理在同一位置）  
                    if (nodes[i].count < LeafSize || depth > 10) {  
                        nodes[i].Add(agent);  
                        nodes[i].count++;  
                        break;  
                    } else {  
                        // 拆分节点  
                        nodes[i].child00 = GetNodeIndex();  
                        nodes[i].Distribute(nodes, r);  
                    }  
                }  
                // 注意，这里没有else子句  
                if (nodes[i].child00 != i) {  
                    // 非叶节点  
                    Vector2 c = r.center;  
                    if (p.x > c.x) {  
                        if (p.y > c.y) {  
                            i = nodes[i].child00 + 3;  
                            r = Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax);  
                        } else {  
                            i = nodes[i].child00 + 2;  
                            r = Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y);  
                        }  
                    } else {  
                        if (p.y > c.y) {  
                            i = nodes[i].child00 + 1;  
                            r = Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax);  
                        } else {  
                            i = nodes[i].child00;  
                            r = Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y);  
                        }  
                    }  
                }  
            }  
        }
        
        public void CalculateSpeeds () {
            nodes[0].CalculateMaxSpeed(nodes, 0);
        }

        public void Query (Vector2 p, float speed, float timeHorizon, float agentRadius, JNRVOAgent agent) {
            new QuadtreeQuery {
                p = p, speed = speed, timeHorizon = timeHorizon, maxRadius = float.PositiveInfinity,
                agentRadius = agentRadius, agent = agent, nodes = nodes
            }.QueryRec(0, bounds);
        }
        
        struct QuadtreeQuery {  
            public Vector2 p;  
            public float speed, timeHorizon, agentRadius, maxRadius;  
            public JNRVOAgent agent;  
            public Node[] nodes;  
  
            public void QueryRec(int i, Rect r) {  
                // 确定需要搜索的半径，以便将所有代理考虑在内  
                // 注意：第二个agentRadius的使用实际上应该是其他代理的半径，而不是此代理的半径  
                // 但是出于性能和简化的考虑，我们假设代理具有大致相同的半径  
                // 因此，具有非常小半径的代理在某些情况下可能会过晚地检测到具有非常大半径的代理  
                // 但是这种效果应该是次要的。  
                var radius = System.Math.Min(System.Math.Max((nodes[i].maxSpeed + speed)*timeHorizon, agentRadius) + agentRadius, maxRadius);  
  
                if (nodes[i].child00 == i) {  
                    // 叶节点  
                    for (JNRVOAgent a = nodes[i].linkedList; a != null; a = a.next) {  
                        float v = agent.InsertAgentNeighbour(a, radius*radius);  
                        // 如果代理达到了附近代理的最大数量阈值，则限制搜索  
                        if (v < maxRadius*maxRadius) {  
                            maxRadius = Mathf.Sqrt(v);  
                        }  
                    }  
                } else {  
                    // 非叶节点  
                    Vector2 c = r.center;  
                    if (p.x-radius < c.x) {  
                        if (p.y-radius < c.y) {  
                            QueryRec(nodes[i].child00, Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y));  
                            radius = System.Math.Min(radius, maxRadius);  
                        }  
                        if (p.y+radius > c.y) {  
                            QueryRec(nodes[i].child00+1, Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax));  
                            radius = System.Math.Min(radius, maxRadius);  
                        }  
                    }  
  
                    if (p.x+radius > c.x) {  
                        if (p.y-radius < c.y) {  
                            QueryRec(nodes[i].child00+2, Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y));  
                            radius = System.Math.Min(radius, maxRadius);  
                        }  
                        if (p.y+radius > c.y) {  
                            QueryRec(nodes[i].child00+3, Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax));  
                        }  
                    }  
                }  
            }  
        }
        
        public void DebugDraw () {
            DebugDrawRec(0, bounds);
        }

        void DebugDrawRec (int i, Rect r) {
            Debug.DrawLine(new Vector3(r.xMin, 0, r.yMin), new Vector3(r.xMax, 0, r.yMin), Color.white);
            Debug.DrawLine(new Vector3(r.xMax, 0, r.yMin), new Vector3(r.xMax, 0, r.yMax), Color.white);
            Debug.DrawLine(new Vector3(r.xMax, 0, r.yMax), new Vector3(r.xMin, 0, r.yMax), Color.white);
            Debug.DrawLine(new Vector3(r.xMin, 0, r.yMax), new Vector3(r.xMin, 0, r.yMin), Color.white);

            if (nodes[i].child00 != i) {
                // Not a leaf node
                Vector2 c = r.center;
                DebugDrawRec(nodes[i].child00+3, Rect.MinMaxRect(c.x, c.y, r.xMax, r.yMax));
                DebugDrawRec(nodes[i].child00+2, Rect.MinMaxRect(c.x, r.yMin, r.xMax, c.y));
                DebugDrawRec(nodes[i].child00+1, Rect.MinMaxRect(r.xMin, c.y, c.x, r.yMax));
                DebugDrawRec(nodes[i].child00+0, Rect.MinMaxRect(r.xMin, r.yMin, c.x, c.y));
            }

            for (JNRVOAgent a = nodes[i].linkedList; a != null; a = a.next) {
                var p = nodes[i].linkedList.position;
                Debug.DrawLine(new Vector3(p.x, 0, p.y)+Vector3.up, new Vector3(a.position.x, 0, a.position.y)+Vector3.up, new Color(1, 1, 0, 0.5f));
            }
        }
        
    }
}