using System;
using System.Collections.Generic;
using System.Linq;
using Global.Shared.OcTree.Data;
using Global.Shared.Plugin;
using Global.Shared.Util;
using VRageMath;

namespace Global.Shared.OcTree
{
    public class GenericOcTree<T> : IPoolable where T : IOcTreeData
    {
        private readonly GenericContainer<T> _bounds;

        private readonly List<GenericContainer<T>> _containersToAdd = new List<GenericContainer<T>>();

        private readonly Pool<GenericContainer<T>> _cPool =
            new Pool<GenericContainer<T>>(() => new GenericContainer<T>());

        private readonly GenericOcTree<T>[] _nodes = new GenericOcTree<T>[8];

        private readonly Pool<GenericOcTree<T>> _otPool = new Pool<GenericOcTree<T>>(() => new GenericOcTree<T>());

        private int _capacity;
        private Bag<GenericContainer<T>> _containers;

        private Dictionary<long, GenericContainer<T>> _idToContainer;
        private int _maxDepth;

        private GenericOcTree<T> _parent;

        private int treeDepth;

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

        public GenericOcTree(float x, float y, float z, float width, float height, float depth, int capacity,
            int maxDepth)
        {
            _bounds = new GenericContainer<T>().Set(x, y, z, width, height, depth);
            _capacity = capacity;
            _maxDepth = maxDepth;
            _containers = new Bag<GenericContainer<T>>(capacity);
            _idToContainer = new Dictionary<long, GenericContainer<T>>();
        }

        public T this[long entityId] => Get(entityId);

        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 GenericOcTree<T> Init(int currentTreeDepth, float x, float y, float z, float width, float height,
            float depth,
            GenericOcTree<T> parent)
        {
            treeDepth = currentTreeDepth;
            _parent = parent;
            _capacity = parent?._capacity ?? 1;
            _maxDepth = parent?._maxDepth ?? 6;
            _bounds.Set(x, y, z, width, height, depth);
            _idToContainer = parent?._idToContainer ?? new Dictionary<long, GenericContainer<T>>();
            _containers = new Bag<GenericContainer<T>>(_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 bool ShouldSplit()
        {
            var halfWidth = _bounds.Width / 2;
            var halfHeight = _bounds.Height / 2;
            var halfDepth = _bounds.Depth / 2;
            var temporaryBounds = new GenericContainer<T>();
            temporaryBounds.Set(_bounds.X, _bounds.Y, _bounds.Z, halfWidth, halfHeight, halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y, _bounds.Z, halfWidth, halfHeight, halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            temporaryBounds.Set(_bounds.X, _bounds.Y + halfHeight, _bounds.Z, halfWidth, halfHeight, halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y + halfHeight, _bounds.Z, halfWidth, halfHeight,
                halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            temporaryBounds.Set(_bounds.X, _bounds.Y, _bounds.Z + halfDepth, halfWidth, halfHeight, halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y, _bounds.Z + halfDepth, halfWidth, halfHeight,
                halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            temporaryBounds.Set(_bounds.X, _bounds.Y + halfHeight, _bounds.Z + halfDepth, halfWidth, halfHeight,
                halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            temporaryBounds.Set(_bounds.X + halfWidth, _bounds.Y + halfHeight, _bounds.Z + halfDepth, halfWidth,
                halfHeight, halfDepth);
            if (_containers.All(e => temporaryBounds.Contains(e))) return false;
            return true;
        }

        private void Split()
        {
            // if (!ShouldSplit()) return; TODO: See if necessary

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

        public IEnumerable<T> GetAll()
        {
            if (_nodes[0] != null)
                foreach (var data in _nodes.SelectMany(e => e.GetAll()))
                    yield return data;
            foreach (var genericContainer in _containers)
                if (genericContainer.Value != null)
                    yield return genericContainer.Value;
        }

        #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 Update(long entityId)
        {
            if (!Contains(entityId)) return;
            var c = _idToContainer[entityId];
            try
            {
                var (removedChildren, addedChildren) = c.Update();
                if (c.EntityId != entityId)
                {
                    var old = entityId;
                    entityId = c.EntityId;
                    // already contains new id.. just give up
                    if (Contains(entityId)) return;
                    // Entity id changed, update the octree
                    _idToContainer.Remove(old);
                    _idToContainer.Add(entityId, c);
                }

                // Attempting to update subgrid
                if (removedChildren != null)
                    foreach (var removedChild in removedChildren)
                        _idToContainer.Remove(removedChild);
                if (addedChildren != null)
                    foreach (var addedChild in addedChildren)
                        _idToContainer.Add(addedChild, c);

                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);
            }
            catch (Exception e)
            {
                GlobalInstance.Log.Critical(e, "Failed to update octree");
            }
        }

        public void Insert(T value)
        {
            Insert(_cPool.Obtain().Set(value));
        }

        private void Insert(GenericContainer<T> 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;
            if (c.ChildIds != null)
                foreach (var childId in c.ChildIds)
                    _idToContainer[childId] = c;
            _containers.Add(c);

            // If under capacity or at/over max depth, we're done.
            if (_containers.Size() <= _capacity || treeDepth >= _maxDepth) return;

            if (_nodes[0] == null) Split();
            if (_nodes[0] == null) return;

            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);
            if (container.ChildIds != null)
                foreach (var childId in container.ChildIds)
                    _idToContainer.Remove(childId);
            container.Value?.Dispose();
            container.Value = default;
            _cPool.Release(container);
            parent?._parent?.HandleMerge();
        }

        #endregion

        #region Getters

        public bool Contains(long entityId)
        {
            return _idToContainer.ContainsKey(entityId);
        }

        public T Get(long entityId)
        {
            return !Contains(entityId) ? default : _idToContainer[entityId].Value;
        }

        /// <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 GenericContainer in _containers
                where GenericContainer.MatchesFlags(flags) && GenericContainer.Overlaps(x, y, z, width, height, depth)
                select GenericContainer.EntityId);

            return entityIds;
        }


        public void GetExact(List<T> outList, BoundingSphereD sphereD, long flags = 0L)
        {
            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;

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

            if (_containers.Any())
                outList.AddRange(from container in _containers
                    where container.MatchesFlags(flags) &&
                          container.Overlaps(x, y, z, width, height, depth) &&
                          sphereD.Intersects(container.BoundingBox)
                    select container.Value);
        }

        public void GetExact(List<T> outList, BoundingBoxD box, long flags = 0L)
        {
            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;

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

            if (_containers.Any())
                outList.AddRange(from container in _containers
                    where container.MatchesFlags(flags) &&
                          container.Overlaps(x, y, z, width, height, depth) &&
                          box.Intersects(container.BoundingBox)
                    select container.Value);
        }

        public void GetExact(List<T> outList, MyOrientedBoundingBoxD boxD, long flags = 0L)
        {
            var box = boxD.GetAABB();
            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;

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

            if (_containers.Any())
                // ReSharper disable once LoopCanBeConvertedToQuery
                foreach (var container in _containers)
                {
                    if (!container.MatchesFlags(flags) || !container.Overlaps(x, y, z, width, height, depth)) continue;
                    var containerBox = container.BoundingBox;
                    if (boxD.Intersects(ref containerBox)) outList.Add(container.Value);
                }
        }


        public IEnumerable<T> GetEnumerable(BoundingSphereD sphereD, long flags = 0L)
        {
            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)) yield break;

            if (_nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE)
                    foreach (var data in _nodes[index].GetEnumerable(sphereD, flags))
                        yield return data;
                else
                    foreach (var data in _nodes.SelectMany(e => e.GetEnumerable(sphereD, flags)))
                        yield return data;
            }

            if (!_containers.Any()) yield break;

            foreach (var c in _containers)
                if (c.MatchesFlags(flags) && c.Overlaps(x, y, z, width, height, depth) &&
                    sphereD.Intersects(c.BoundingBox))
                    yield return c.Value;
        }


        public IEnumerable<T> GetEnumerable(BoundingBoxD box, long flags = 0L)
        {
            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)) yield break;

            if (_nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE)
                    foreach (var data in _nodes[index].GetEnumerable(box, flags))
                        yield return data;
                else
                    foreach (var data in _nodes.SelectMany(e => e.GetEnumerable(box, flags)))
                        yield return data;
            }

            if (!_containers.Any()) yield break;

            foreach (var c in _containers)
                if (c.MatchesFlags(flags) && c.Overlaps(x, y, z, width, height, depth) && box.Intersects(c.BoundingBox))
                    yield return c.Value;
        }


        public IEnumerable<T> GetEnumerable(MyOrientedBoundingBoxD boxD, long flags = 0L)
        {
            var box = boxD.GetAABB();
            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)) yield break;

            if (_nodes[0] != null)
            {
                var index = IndexOf(x, y, z, width, height, depth);
                if (index != OUTSIDE)
                    foreach (var data in _nodes[index].GetEnumerable(boxD, flags))
                        yield return data;
                else
                    foreach (var data in _nodes.SelectMany(e => e.GetEnumerable(boxD, flags)))
                        yield return data;
            }

            if (!_containers.Any()) yield break;

            foreach (var c in _containers)
            {
                var cBox = c.BoundingBox;
                if (c.MatchesFlags(flags) && c.Overlaps(x, y, z, width, height, depth) && boxD.Intersects(ref cBox))
                    yield return c.Value;
            }
        }

        #endregion
    }

    public class GenericContainer<T> : IPoolable, IContainer where T : IOcTreeData
    {
        private static readonly BoundingBoxD ReusableBox = new BoundingBoxD();
        public List<long> ChildIds;
        public long EntityId;

        public long Flags;
        public GenericOcTree<T> Parent;

        public T Value;

        public BoundingBoxD BoundingBox => ReusableBox.SetToContainer(this);
        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 void Reset()
        {
            ChildIds = null;
            EntityId = -1;
            Flags = 0;
            X = 0;
            Y = 0;
            Z = 0;
            Width = 0;
            Height = 0;
            Depth = 0;
            Value = default;
        }

        public GenericContainer<T> Set(T value)
        {
            Value = value;
            ChildIds = value.ChildIds;
            EntityId = value.Id;
            Flags = value.Flags;
            var box = value.BoundingBox;
            X = (float)box.Min.X;
            Y = (float)box.Min.Y;
            Z = (float)box.Min.Z;
            Width = (float)box.Max.X - X;
            Height = (float)box.Max.Y - Y;
            Depth = (float)box.Max.Z - Z;
            return this;
        }

        public KeyValuePair<IEnumerable<long>, IEnumerable<long>> Update()
        {
            if (Value == null) return new KeyValuePair<IEnumerable<long>, IEnumerable<long>>(null, null);
            Value.Update();
            var previousChildIds = ChildIds;
            ChildIds = Value.ChildIds;
            Flags = Value.Flags;
            var box = Value.BoundingBox;
            X = (float)box.Min.X;
            Y = (float)box.Min.Y;
            Z = (float)box.Min.Z;
            Width = (float)box.Max.X - X;
            Height = (float)box.Max.Y - Y;
            Depth = (float)box.Max.Z - Z;
            return new KeyValuePair<IEnumerable<long>, IEnumerable<long>>(
                previousChildIds.Where(e => !ChildIds.Contains(e)),
                ChildIds.Where(e => !previousChildIds.Contains(e)));
        }

        public GenericContainer<T> 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 == 0 || (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)
        {
            return !(X > x + width || x > X + Width || Y > y + height || y > Y + Height || Z > z + depth ||
                     z > Z + Depth);
        }

        public bool IntersectsRay(Vector3 start, Vector3 end)
        {
            var lineD = new LineD(start, end);
            var box = BoundingBox;
            return box.Intersects(ref lineD);
        }

        public bool Contains(GenericContainer<T> c)
        {
            return Contains(c.X, c.Y, c.Z, c.Width, c.Height, c.Depth);
        }
    }
}