using System;
using System.Buffers;
using System.Collections;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using System.Net.Http.Headers;
using System.Security.Policy;
using System.Threading;
using System.Threading.Tasks;
using BITKit;
#if UNITY_5_3_OR_NEWER
using Unity.Burst;
#endif
using Unity.Mathematics;

namespace Net.BITKit.Quadtree
{
    public class Quadtree
    {
        public QuadtreeNode Root { get; private set; }

        public IDictionary<int, float2> Positions
        {
            get
            {
                Expansion();
                return _positions;
            }
        }

        public IDictionary<int, float2> Sizes => _sizes;
        private readonly Dictionary<int, float2> _positions;
        private readonly ConcurrentQueue<(int, float2)> _queue;
        private readonly Dictionary<int, float2> _sizes;

        public Quadtree(float2 center, float2 size)
        {
            _sizes = new Dictionary<int, float2>();
            _positions = new Dictionary<int, float2>();
            _queue = new ConcurrentQueue<(int, float2)>();
            Root = new QuadtreeNode(this, center, size);
        }

        public void Insert(in int objectId, in float2 position, in float2 size = default)
        {
            _queue.Enqueue((objectId, position));

            if (size.x is not 0)
            {
                _sizes.TryAdd(objectId, size);
            }
            else
            {
                var root = Root;

                InsertRecursive(ref root, in objectId, in position);
            }
        }

        private void InsertRecursive(ref QuadtreeNode node, in int objectId, in float2 position)
        {
            if (!node.Contains(position))
                return;
            if (node.Objects.Count < 4 || node.Size.x <= 1f || node.Depth > Root.Size.x / 8) // 假设最小节点大小为1
            {
                node.Objects.Add(objectId);
            }
            else
            {
                if (node.Children[0].Size.x == 0) // 如果子节点未初始化
                {
                    node.Split();
                }

                for (var i = 0; i < 4; i++)
                {
                    InsertRecursive(ref node.Children[i], objectId, position);
                }
            }
        }

        public Span<int> Query(float2 position, float radius)
        {
            Expansion();

            var index = 0;

            var root = Root;

            var pool = ArrayPool<int>.Shared;

            var array = pool.Rent(math.ceilpow2(_positions.Count * 2));

            pool.Return(array);

            QueryRecursive(in root, in position, in radius, array.AsSpan(), ref index);
            foreach (var (key, size) in _sizes)
            {
                var pos = _positions[key];

                if (IsCircleInRect(pos, radius, position, size))
                {
                    array[index] = key;
                    index++;
                }
            }

            try
            {
                return array.AsSpan()[..index];
            }
            finally
            {
                pool.Return(array);
            }
        }
#if UNITY_5_3_OR_NEWER
        [BurstCompile]
#endif
        private static bool IsCircleInRect(float2 point, float radius, float2 pos, float2 size)
        {
            var halfSize = size * 0.5f;
            var min = pos - halfSize; // 矩形左下角
            var max = pos + halfSize; // 矩形右上角

            // 计算扩展后的包围盒
            var expandedMin = min - radius;
            var expandedMax = max + radius;

            // 检查点是否在扩展的矩形内
            return point.x >= expandedMin.x && point.x <= expandedMax.x &&
                   point.y >= expandedMin.y && point.y <= expandedMax.y;
        }

        private void Expansion()
        {
            while (_queue.TryDequeue(out var item))
            {
                _positions.TryAdd(item.Item1, item.Item2);
            }
        }
#if UNITY_5_3_OR_NEWER
        [BurstCompile]
#endif
        private void QueryRecursive(in QuadtreeNode node, in float2 position, in float radius, Span<int> result,
            ref int index)
        {
            if (!Intersects(node.Center, node.Size, position, radius))
                return;



            foreach (var obj in node.Objects)
            {
                // 直接 TryGetValue，避免 `Positions[obj]` 可能的重复哈希查找
                if (!_positions.TryGetValue(obj, out var objPos)) continue;

                // 计算平方距离，避免额外变量
                if (math.dot(objPos - position, objPos - position) > radius * radius) continue;

                // 直接写入 result
                try
                {
                    result[index] = obj;
                }
                catch (Exception)
                {
                    #if UNITY_EDITOR
                    var unions = result.ToArray().Distinct();
                    var hashSet = new HashSet<int>();
                    For(Root);
                     void For(in QuadtreeNode node)
                    {
                        foreach (var child in node.Children)
                        {
                            For(child);
                        }
                        foreach (var x in node.Objects)
                        {
                            if (hashSet.Add(x) is false)
                            {
                                
                            }
                        }
                    }
                    #endif
                    throw;
                }

                index++;
            }


            for (var i = 0; i < 4; i++)
            {
                if (node.Children[i].Size.x > 0)
                {
                    QueryRecursive(in node.Children[i], in position, in radius, result, ref index);
                }
            }
        }
#if UNITY_5_3_OR_NEWER
        [BurstCompile]
#endif
        private static bool Intersects(in float2 center, in float2 size, in float2 position, in float radius)
        {
            // 计算 AABB 的最小/最大点
            var min = center - size * 0.5f;
            var max = center + size * 0.5f;

            // 找到圆心到 AABB 的最近点
            var closest = math.clamp(position, min, max);

            // 计算圆心到最近点的距离
            var delta = position - closest;
            var distanceSq = math.dot(delta, delta);

            // 相交条件：如果距离平方 <= 半径平方
            return distanceSq <= (radius * radius);
        }


        // 删除对象
        public bool Remove(int objectId)
        {
            Expansion();

            if (_positions.TryGetValue(objectId, out var position) is false) return false;

            _sizes.TryRemove(objectId);

            var root = Root;

            if (RemoveRecursive(ref root, objectId, position) is false) return false;
            _positions.Remove(objectId);


            return true;
        }
        private static bool RemoveRecursive(ref QuadtreeNode node, int objectId, float2 position)
        {
            if (!node.Contains(position))
                return false;

            // 尝试从当前节点删除
            bool removed = node.Objects.Remove(objectId);

            // 如果当前节点是叶子节点，返回是否成功删除
            if (node.IsLeaf())
            {
                return removed;
            }

            // 递归从子节点删除
            for (var i = 0; i < 4; i++)
            {
                if (RemoveRecursive(ref node.Children[i], objectId, position))
                {
                    removed = true;
                }
            }

            // 如果对象被移除，尝试合并子节点
            if (removed)
            {
                TryMerge(ref node);
            }

            return removed;
        }

        private static void TryMerge(ref QuadtreeNode node)
        {
            if (node.IsLeaf()) return;

            // 检查是否可以合并子节点
            var totalObjects = node.Objects.Count;
            for (var i = 0; i < 4; i++)
            {
                totalObjects += node.Children[i].Objects.Count;
            }

            // 如果当前节点和所有子节点的对象数量小于等于阈值，则合并
            if (totalObjects <= 4)
            {
                // 把所有子节点的对象都合并到当前节点
                foreach (var child in node.Children)
                {
                    node.Objects.UnionWith(child.Objects);
                    child.Objects.Clear(); // 清空子节点的对象集合
                }

                // 清空子节点，重新初始化
                for (var i = 0; i < 4; i++)
                {
                    node.Children[i] = new QuadtreeNode()
                    {
                        Depth = node.Depth + 1
                    };
                }
            }
        }
    }
}