using System.Collections.Generic;
using System.Linq;
using VRageMath;

namespace Global.Shared.OcTree
{
    public class OcTree : IPoolable
    {
        private static int TotalIds;
        private static readonly Pool<Container> cPool = new Pool<Container>(() => new Container());
        private static readonly Pool<OcTree> otPool = new Pool<OcTree>(() => new OcTree());

        private readonly Container _bounds;

        private readonly List<Container> _containersToAdd = new List<Container>();
        private readonly OcTree[] nodes = new OcTree[8];
        private Bag<Container> _containers;

        private int capacity;

        private Dictionary<long, Container> idToContainer;
        private int maxDepth;

        private OcTree parent;

        private BoundingBoxD reusableBox = new BoundingBoxD();
        private int treeDepth;

        private OcTree() : this(0, 0, 0, 0, 0, 0, 0, 0)
        {
        }

        public OcTree(float x, float y, float z, float width, float height, float depth, int capacity,
            int maxDepth)
        {
            _bounds = new Container().Set(x, y, z, width, height, depth);
            this.capacity = capacity;
            this.maxDepth = maxDepth;
            _containers = new Bag<Container>(capacity);
            idToContainer = new Dictionary<long, Container>();
        }

        public void Reset()
        {
            for (var i = _containers.Size() - 1; i >= 0; i--) cPool.Release(_containers.Remove(i));
            for (var i = 0; i < nodes.Length; i++)
                if (nodes[i] != null)
                {
                    otPool.Release(nodes[i]);
                    nodes[i] = null;
                }
        }

        private OcTree Init(int currentTreeDepth, float x, float y, float z, float width, float height, float depth,
            OcTree parent)
        {
            treeDepth = currentTreeDepth;
            this.parent = parent;
            capacity = parent?.capacity ?? 1;
            maxDepth = parent?.maxDepth ?? 6;
            _bounds.Set(x, y, z, width, height, depth);
            idToContainer = parent?.idToContainer ?? new Dictionary<long, Container>();
            _containers = new Bag<Container>(capacity);

            return this;
        }

        private int IndexOf(float x, float y, float z, float width, float height, float depth)
        {
            var midX = _bounds.X + _bounds.Width / 2;
            var midY = _bounds.Y + _bounds.Height / 2;
            var midZ = _bounds.Z + _bounds.Depth / 2;

            var res = 0;
            if (x > midX) res |= E;
            else if (x < midX && x + width < midX) res |= W;
            else return OUTSIDE;

            if (y > midY) res |= U;
            else if (y < midY && y + height < midY) res |= D;
            else return OUTSIDE;

            if (z > midZ) res |= S;
            else if (z < midZ && z + depth < midZ) res |= N;
            else return OUTSIDE;

            return res;
        }

        private bool ShouldMerge()
        {
            // If has no children, has nothing to merge
            if (nodes[0] == null) return false;
            // If has any children with children, cannot merge
            if (nodes.Any(n => n.nodes[0] != null)) return false;
            // If children combined contain less than capacity, can merge
            return _containers.Size() + nodes.Sum(n => n._containers.Size()) <= capacity;
        }

        private void Split()
        {
            var halfWidth = _bounds.Width / 2;
            var halfHeight = _bounds.Height / 2;
            var halfDepth = _bounds.Depth / 2;
            nodes[DNW] = otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y, _bounds.Z, halfWidth, halfHeight,
                halfDepth, this);
            nodes[DNE] = otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y, _bounds.Z, halfWidth,
                halfHeight, halfDepth, this);
            nodes[DSW] = otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y, _bounds.Z + halfDepth, halfWidth,
                halfHeight, halfDepth, this);
            nodes[DSE] = otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y,
                _bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth, this);

            nodes[UNW] = otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y + halfHeight, _bounds.Z,
                halfWidth, halfHeight, halfDepth, this);
            nodes[UNE] = otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y + halfHeight,
                _bounds.Z, halfWidth, halfHeight, halfDepth, this);
            nodes[USW] = otPool.Obtain().Init(treeDepth + 1, _bounds.X, _bounds.Y + halfHeight,
                _bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth, this);
            nodes[USE] = otPool.Obtain().Init(treeDepth + 1, _bounds.X + halfWidth, _bounds.Y + halfHeight,
                _bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth, this);
        }

        private void HandleMerge()
        {
            if (!ShouldMerge()) return;

            for (var index = nodes.Length - 1; index >= 0; index--)
            {
                var ocTree = nodes[index];
                for (var i = ocTree._containers.Size() - 1; i >= 0; i--)
                {
                    var ocTreeContainer = ocTree._containers[i];
                    _containersToAdd.Add(ocTreeContainer);
                    ocTree._containers.Remove(i);
                }
            }

            for (var index = nodes.Length - 1; index >= 0; index--)
            {
                var ocTree = nodes[index];
                nodes[index] = null;
                otPool.Release(ocTree);
                ocTree.Reset();
            }

            foreach (var container in _containersToAdd) Insert(container);

            _containersToAdd.Clear();
        }

        #region Node Bitwise

        private const int OUTSIDE = -1;
        private const int D = 0b000;
        private const int U = 0b100;
        private const int S = 0b000;
        private const int N = 0b010;
        private const int W = 0b000;
        private const int E = 0b001;
        private const int DSW = D | S | W; // 0
        private const int DSE = D | S | E; // 1
        private const int DNW = D | N | W; // 2
        private const int DNE = D | N | E; // 3
        private const int USW = U | S | W; // 4
        private const int USE = U | S | E; // 5
        private const int UNW = U | N | W; // 6
        private const int UNE = U | N | E; // 7

        #endregion

        #region Modifiers

        public void Upsert(long entityId, long flags, BoundingBoxD aabb)
        {
            Upsert(entityId, flags, (float)aabb.Min.X, (float)aabb.Min.Y, (float)aabb.Min.Z,
                (float)aabb.Max.X - (float)aabb.Min.X, (float)aabb.Max.Y - (float)aabb.Min.Y,
                (float)aabb.Max.Z - (float)aabb.Min.Z);
        }

        public void Upsert(long entityId, float x, float y, float z, float width, float height, float depth)
        {
            Upsert(entityId, 0L, x, y, z, width, height, depth);
        }

        public void Upsert(long entityId, long flags, float x, float y, float z, float width, float height, float depth)
        {
            Container container = null;
            if (idToContainer.ContainsKey(entityId)) container = idToContainer[entityId];
            if (container != null && container.EntityId != -1)
            {
                container.Flags |= flags;
                Update(entityId, x, y, z, width, height, depth);
                return;
            }

            Insert(entityId, flags, x, y, z, width, height, depth);
        }

        private void Update(long entityId, float x, float y, float z, float width, float height, float depth)
        {
            var c = idToContainer[entityId];
            c.Set(entityId, c.Flags, x, y, z, width, height, depth);

            var parentTree = c.Parent;
            if (parentTree._bounds.Contains(c)) return;

            parentTree._containers.Remove(c);
            while (parentTree.parent != null && !parentTree._bounds.Contains(c)) parentTree = parentTree.parent;

            parentTree.Insert(c);
        }

        private void Insert(long entityId, float x, float y, float z, float width, float height, float depth)
        {
            Insert(entityId, 0L, x, y, z, width, height, depth);
        }

        private void Insert(long entityId, long flags, float x, float y, float z, float width, float height,
            float depth)
        {
            Insert(cPool.Obtain().Set(entityId, flags, x, y, z, width, height, depth));
        }

        private void Insert(Container c)
        {
            if (nodes[0] != null)
            {
                var index = IndexOf(c.X, c.Y, c.Z, c.Width, c.Height, c.Depth);
                if (index != OUTSIDE)
                {
                    nodes[index].Insert(c);
                    return;
                }
            }

            c.Parent = this;
            idToContainer[c.EntityId] = c;
            _containers.Add(c);

            if (_containers.Size() <= capacity || treeDepth >= maxDepth) return;

            if (nodes[0] == null) Split();

            var items = _containers.data;
            for (var i = _containers.Size() - 1; i >= 0; i--)
            {
                var next = items[i];
                if (next == null) continue;
                var index = IndexOf(next.X, next.Y, next.Z, next.Width, next.Height, next.Depth);
                if (index == OUTSIDE) continue;
                nodes[index].Insert(next);
                _containers.Remove(i);
            }
        }

        public void Remove(long entityId)
        {
            if (!idToContainer.ContainsKey(entityId)) return;

            var container = idToContainer[entityId];
            var parent = container.Parent;
            parent?._containers.Remove(container);
            idToContainer.Remove(entityId);
            cPool.Release(container);
            parent?.parent?.HandleMerge();
        }

        #endregion

        #region Getters

        /// <summary>
        ///     Returns entityIds of entities that are inside the OcTrees that contain given point.
        /// </summary>
        /// <param name="entityIds">List to fill</param>
        /// <param name="x">X position</param>
        /// <param name="y">Y position</param>
        /// <param name="z">Z position</param>
        public List<long> Get(List<long> entityIds, float x, float y, float z)
        {
            if (!_bounds.Contains(x, y, z)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, 0f, 0f, 0f);
                if (index != OUTSIDE) nodes[index].Get(entityIds, x, y, z);
            }

            entityIds.AddRange(_containers.Select(container => container.EntityId));

            return entityIds;
        }

        public List<long> Get(List<long> entityIds, float x, float y, float z, long flags)
        {
            if (flags == 0L) return Get(entityIds, x, y, z);
            if (!_bounds.Contains(x, y, z)) return entityIds;
            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, 0f, 0f, 0f);
                if (index != OUTSIDE) nodes[index].Get(entityIds, x, y, z, flags);
            }

            entityIds.AddRange(from container in _containers
                where container.MatchesFlags(flags)
                select container.EntityId
            );

            return entityIds;
        }

        public List<long> Get(List<long> entityIds, BoundingSphereD sphereD)
        {
            var box = sphereD.GetBoundingBox();
            var x = (float)box.Min.X;
            var y = (float)box.Min.Y;
            var z = (float)box.Min.Z;
            var width = (float)box.Max.X - x;
            var height = (float)box.Max.Y - y;
            var depth = (float)box.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].Get(entityIds, sphereD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.Get(entityIds, sphereD);
            }

            entityIds.AddRange(_containers.Select(container => container.EntityId));

            return entityIds;
        }

        public List<long> Get(List<long> entityIds, long flags, BoundingSphereD sphereD)
        {
            if (flags == 0L) return Get(entityIds, sphereD);
            var box = sphereD.GetBoundingBox();
            var x = (float)box.Min.X;
            var y = (float)box.Min.Y;
            var z = (float)box.Min.Z;
            var width = (float)box.Max.X - x;
            var height = (float)box.Max.Y - y;
            var depth = (float)box.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].Get(entityIds, flags, sphereD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.Get(entityIds, flags, sphereD);
            }

            entityIds.AddRange(from container in _containers
                where container.MatchesFlags(flags)
                select container.EntityId);

            return entityIds;
        }

        //TODO: Make exact check sphere rather than box
        public List<long> GetExact(List<long> entityIds, BoundingSphereD sphereD)
        {
            var box = sphereD.GetBoundingBox();
            var x = (float)box.Min.X;
            var y = (float)box.Min.Y;
            var z = (float)box.Min.Z;
            var width = (float)box.Max.X - x;
            var height = (float)box.Max.Y - y;
            var depth = (float)box.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].GetExact(entityIds, sphereD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.GetExact(entityIds, sphereD);
            }

            entityIds.AddRange(from container in _containers
                where container.Overlaps(x, y, z, width, height, depth) && sphereD.Intersects(
                    new BoundingBoxD(new Vector3D(container.X, container.Y, container.Z),
                        new Vector3D(
                            container.X + container.Width,
                            container.Y + container.Height,
                            container.Z + container.Depth
                        )))
                select container.EntityId);

            return entityIds;
        }

        public List<long> GetExact(List<long> entityIds, long flags, BoundingSphereD sphereD)
        {
            if (flags == 0L) return GetExact(entityIds, sphereD);
            var box = sphereD.GetBoundingBox();
            var x = (float)box.Min.X;
            var y = (float)box.Min.Y;
            var z = (float)box.Min.Z;
            var width = (float)box.Max.X - x;
            var height = (float)box.Max.Y - y;
            var depth = (float)box.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].GetExact(entityIds, flags, sphereD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.GetExact(entityIds, flags, sphereD);
            }

            entityIds.AddRange(from container in _containers
                where container.MatchesFlags(flags) && container.Overlaps(x, y, z, width, height, depth) &&
                      sphereD.Intersects(
                          new BoundingBoxD(new Vector3D(container.X, container.Y, container.Z),
                              new Vector3D(
                                  container.X + container.Width,
                                  container.Y + container.Height,
                                  container.Z + container.Depth
                              )))
                select container.EntityId);

            return entityIds;
        }

        public List<long> Get(List<long> entityIds, BoundingBoxD boxD)
        {
            var x = (float)boxD.Min.X;
            var y = (float)boxD.Min.Y;
            var z = (float)boxD.Min.Z;
            var width = (float)boxD.Max.X - x;
            var height = (float)boxD.Max.Y - y;
            var depth = (float)boxD.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].Get(entityIds, boxD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.Get(entityIds, boxD);
            }

            entityIds.AddRange(_containers.Select(container => container.EntityId));

            return entityIds;
        }

        public List<long> Get(List<long> entityIds, long flags, BoundingBoxD boxD)
        {
            if (flags == 0L) return Get(entityIds, boxD);
            var x = (float)boxD.Min.X;
            var y = (float)boxD.Min.Y;
            var z = (float)boxD.Min.Z;
            var width = (float)boxD.Max.X - x;
            var height = (float)boxD.Max.Y - y;
            var depth = (float)boxD.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].Get(entityIds, flags, boxD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.Get(entityIds, flags, boxD);
            }

            entityIds.AddRange(from container in _containers
                where container.MatchesFlags(flags) && container.Overlaps(x, y, z, width, height, depth)
                select container.EntityId);

            return entityIds;
        }

        public List<long> GetExact(List<long> entityIds, MyOrientedBoundingBoxD boxBoundingBox)
        {
            var boxD = boxBoundingBox.GetAABB();
            var x = (float)boxD.Min.X;
            var y = (float)boxD.Min.Y;
            var z = (float)boxD.Min.Z;
            var width = (float)boxD.Max.X - x;
            var height = (float)boxD.Max.Y - y;
            var depth = (float)boxD.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].GetExact(entityIds, boxD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.GetExact(entityIds, boxD);
            }

            foreach (var container in _containers)
            {
                if (!container.Overlaps(x, y, z, width, height, depth)) continue;

                var containerBox = container.BoundingBox;
                if (boxBoundingBox.Intersects(ref containerBox)) entityIds.Add(container.EntityId);
            }

            return entityIds;
        }

        public List<long> GetExact(List<long> entityIds, long flags, MyOrientedBoundingBoxD boxBoundingBox)
        {
            if (flags == 0L) return GetExact(entityIds, boxBoundingBox);
            var boxD = boxBoundingBox.GetAABB();
            var x = (float)boxD.Min.X;
            var y = (float)boxD.Min.Y;
            var z = (float)boxD.Min.Z;
            var width = (float)boxD.Max.X - x;
            var height = (float)boxD.Max.Y - y;
            var depth = (float)boxD.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].GetExact(entityIds, boxD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.GetExact(entityIds, boxD);
            }

            foreach (var container in _containers)
            {
                if (!container.MatchesFlags(flags) || !container.Overlaps(x, y, z, width, height, depth)) continue;

                var containerBox = container.BoundingBox;
                if (boxBoundingBox.Intersects(ref containerBox)) entityIds.Add(container.EntityId);
            }

            return entityIds;
        }

        public List<long> GetExact(List<long> entityIds, BoundingBoxD boxD)
        {
            var x = (float)boxD.Min.X;
            var y = (float)boxD.Min.Y;
            var z = (float)boxD.Min.Z;
            var width = (float)boxD.Max.X - x;
            var height = (float)boxD.Max.Y - y;
            var depth = (float)boxD.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].GetExact(entityIds, boxD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.GetExact(entityIds, boxD);
            }

            entityIds.AddRange(from container in _containers
                where container.Overlaps(x, y, z, width, height, depth)
                select container.EntityId);

            return entityIds;
        }

        public List<long> GetExact(List<long> entityIds, long flags, BoundingBoxD boxD)
        {
            if (flags == 0L) return GetExact(entityIds, boxD);
            var x = (float)boxD.Min.X;
            var y = (float)boxD.Min.Y;
            var z = (float)boxD.Min.Z;
            var width = (float)boxD.Max.X - x;
            var height = (float)boxD.Max.Y - y;
            var depth = (float)boxD.Max.Z - z;

            if (!_bounds.Overlaps(x, y, z, width, height, depth)) return entityIds;

            if (nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE) nodes[index].GetExact(entityIds, flags, boxD);
                else
                    foreach (var ocTree in nodes)
                        ocTree.GetExact(entityIds, flags, boxD);
            }

            entityIds.AddRange(from container in _containers
                where container.MatchesFlags(flags) && container.Overlaps(x, y, z, width, height, depth)
                select container.EntityId);

            return entityIds;
        }

        #endregion
    }

    public class Container : IPoolable
    {
        public long EntityId;
        public long Flags;
        public OcTree Parent;
        public float X { get; private set; }
        public float Y { get; private set; }
        public float Z { get; private set; }
        public float Width { get; private set; }
        public float Height { get; private set; }
        public float Depth { get; private set; }

        public BoundingBoxD BoundingBox =>
            new BoundingBoxD(new Vector3(X, Y, Z), new Vector3(X + Width, Y + Height, Z + Depth));

        public void Reset()
        {
            EntityId = -1;
            Flags = 0;
            X = 0;
            Y = 0;
            Z = 0;
            Width = 0;
            Height = 0;
            Depth = 0;
        }

        public Container Set(long entityId, long flags, float x, float y, float z, float width, float height,
            float depth)
        {
            EntityId = entityId;
            Flags = flags;
            X = x;
            Y = y;
            Z = z;
            Width = width;
            Height = height;
            Depth = depth;
            return this;
        }

        public Container Set(float x, float y, float z, float width, float height, float depth)
        {
            X = x;
            Y = y;
            Z = z;
            Width = width;
            Height = height;
            Depth = depth;
            return this;
        }

        public bool MatchesFlags(long flags)
        {
            return (Flags & flags) > 0;
        }

        public bool Contains(float x, float y, float z)
        {
            return X <= x && x <= X + Width &&
                   Y <= y && y <= Y + Height &&
                   Z <= z && z <= Z + Depth;
        }

        public bool Contains(float x, float y, float z, float width, float height, float depth)
        {
            var xMax = x + width;
            var yMax = x + height;
            var zMax = x + depth;

            return x > X && x < X + Width && xMax > X && xMax < X + Width &&
                   y > Y && y < Y + Height && yMax > Y && yMax < Y + Height &&
                   z > Z && z < Z + Depth && zMax > Z && zMax < Z + Depth;
        }

        public bool Overlaps(float x, float y, float z, float width, float height, float depth)
        {
            if (X > x + width) return false;
            if (x > X + Width) return false;
            if (Y > y + height) return false;
            if (y > Y + Height) return false;
            if (Z > z + depth) return false;
            if (z > Z + Depth) return false;

            return true;
        }

        public bool Contains(Container c)
        {
            return Contains(c.X, c.Y, c.Z, c.Width, c.Height, c.Depth);
        }
    }
}