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;
using kcp2k;
#if UNITY_5_3_OR_NEWER
using Unity.Burst;
#endif
using Unity.Mathematics;

namespace Net.BITKit.Quadtree
{
    public class Quadtree
    {
        public ref QuadtreeNode Root => ref _root;
        private QuadtreeNode _root;

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

        public readonly int MaxDepth;
        
        public IDictionary<int, float2> Sizes => _sizes;
        private readonly Dictionary<int, float2> _positions;
        private readonly ConcurrentQueue<(int, float2)> _queue;
        private readonly Dictionary<int, float2> _sizes;
        private readonly  ArrayPool<int> _pool=ArrayPool<int>.Create();
        //internal readonly Pool<HashSet<int>> HashSetPool=new(Activator.CreateInstance<HashSet<int>>,null,64);
        private int _queryCount;

        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);

            MaxDepth = (int)math.log2(math.max(size.x, size.y));
        }

        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
            {
                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 is { Objects : {Count: < 4}, IsLeaf: true } || node.Depth>= MaxDepth) // 是叶子节点,并且数量小于4
            {
                node.Objects.Add(objectId);
            }
            else
            {
                if (node.IsLeaf)
                    node.Split();

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

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

            Expansion();

            var index = 0;

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

            QueryRecursive(ref _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++;
                }
            }

            /*
            if (index > _positions.Count+1)
            {
                throw new InvalidOperationException($"Query Count:{_queryCount},size:{_positions.Count},Query result length is {index}");
            }
            */
            _pool.Return(array);
            return array.AsSpan()[..index];
        }
#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(ref QuadtreeNode node, in float2 position, in float radius, Span<int> result,
            ref int index)
        {
            if (!Intersects(node.Center, node.Size, position, radius))
                return;

            if (node.IsLeaf)
            {
                foreach (var obj in node.Objects)
                {
                    var objPos = _positions[obj];

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

                    result[index] = obj;

                    index++;
                }
            }
            else
            {
                for (var i = 0; i < 4; i++)
                {
                    QueryRecursive(ref 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;

            if (RemoveRecursive(ref _root, objectId, position) is false) return false;
            
            _positions.Remove(objectId);
            _sizes.TryRemove(objectId);
            return true;
        }
        private bool RemoveRecursive(ref QuadtreeNode node, int objectId, float2 position)
        {
            if (!node.Contains(position))
                return false;

            // 尝试从当前节点删除
            var 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)) continue;
                removed = true;
                break;
            }

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

            return removed;
        }

        public void Clear()
        {
            _positions.Clear();
            _sizes.Clear();
            _root = new QuadtreeNode(this,default,_root.Size);
        }

        public int Depth
        {
            get
            {
                var depth = 0;
                Expansion();
                For(ref _root);
                return depth;
                void For(ref QuadtreeNode node)
                {
                    if (node.IsLeaf)
                    {
                        depth = math.max(node.Depth, depth);
                        return;
                    }

                    for (var index = 0; index < node.Children.Length; index++)
                    {
                        ref var child =ref node.Children[index];
                        For(ref child);
                    }
                }
            }
        }

        private void TryMerge(ref QuadtreeNode node)
        {
            //Merge has some issue,just disable is
            return;
            if (node.IsLeaf) return;

            var totalObjects = 0;
            for (var i = 0; i < node.Children.Length; i++)
            {
                totalObjects += node.Children[i].Objects.Count;
            }
            // 如果当前节点和所有子节点的对象数量小于等于阈值，则合并
            if (totalObjects <= 4) return;
            // 把所有子节点的对象都合并到当前节点
            for (var i = 0; i < node.Children.Length; i++)
            {
                ref var child = ref node.Children[i];
                node.Objects.UnionWith(child.Objects);
                node.Children[i] = default;
            }
            node.IsLeaf = true;
        }
    }
}