local X={LeftBottomBack=1;LeftBottomFront=2;LeftTopBack=3;LeftTopFront=4,RightBottomBack=5,RightBottomFront=6,RightTopBack=7;RightTopFront=8}local I={[X.LeftBottomBack]={1,1,1};[X.LeftBottomFront]={1;1,2},[X.LeftTopBack]={1;2,1},[X.LeftTopFront]={1;2,2};[X.RightBottomBack]={2;1,1},[X.RightBottomFront]={2;1;2};[X.RightTopBack]={2;2;1},[X.RightTopFront]={2;2;2}}local d={[1]={[1]={[1]=X.LeftBottomBack;[2]=X.LeftBottomFront};[2]={[1]=X.LeftTopBack;[2]=X.LeftTopFront}};[2]={[1]={[1]=X.RightBottomBack;[2]=X.RightBottomFront};[2]={[1]=X.RightTopBack;[2]=X.RightTopFront}}}local function T(X,I,d)return(X-I).Magnitude<=d end local function Q(X,I)local T=I.X<=X.X and 1 or 2 local Q=I.Y<=X.Y and 1 or 2 local v=I.Z<=X.Z and 1 or 2 return d[T][Q][v]end local v=require(script.OctreeLeaf)local D={}D.__index=D D.ClassName=script.Name function D.new(X,I,d,T)assert(typeof(X)=="\086\101\099\116\111\114\051","\078\101\101\100 \118\101\099\116\111\114\051 \099\111\114\110\101\114\095\049")assert(typeof(I)=="\086\101\099\116\111\114\051","\078\101\101\100 \118\101\099\116\111\114\051 \099\111\114\110\101\114\095\050")local Q=setmetatable({},D)Q._children={}Q._corner1=X Q._corner2=I Q._center=(I-X)*.5+X Q._parent=T Q._epsilon=d or.1 Q._dataToLeaf={}if T then assert(T.ClassName==D.ClassName,"\084\114\105\101\100 \116\111 \115\101\116 \105\110\118\097\108\105\100 \112\097\114\101\110\116\033")Q._dataToLeaf=T._dataToLeaf end return Q end function D.Destroy(X)for I,d in pairs(X._children)do d:Destroy()X._children[I]=nil end X._children=nil X._corner1=nil X._corner2=nil X._center=nil X._parent=nil X._epsilon=nil X._data=nil X._coordinate=nil X._dataToLeaf=nil end function D.Insert(d,X,I)assert(typeof(X)=="\086\101\099\116\111\114\051","\067\111\111\114\100\105\110\097\116\101\115 \109\117\115\116 \098\101 \086\101\099\116\111\114\051\033")assert(I,"\084\114\105\101\100 \116\111 \105\110\115\101\114\116 \119\105\116\104\111\117\116 \100\097\116\097\033")if(d._center-X).Magnitude<=d._epsilon then d._data=I d._coordinate=X d._dataToLeaf[I]=d return d end local T=Q(d._center,X)local M=d._children[T]if not M then d._children[T]=v.new(X,I,d)d._dataToLeaf[I]=d._children[T]elseif M.ClassName==v.ClassName then if(M._coordinate-X).Magnitude<=d._epsilon then error("\084\114\105\101\100 \116\111 \105\110\115\101\114\116 \100\117\112\108\105\099\097\116\101 \112\111\105\110\116 \105\110 \111\099\116\114\101\101\033")end local Q,v=d:GetBounds(T)local J=D.new(Q,v,d._epsilon,d)d._children[T]=J J:Insert(M._coordinate,M._data)return J:Insert(X,I)else return M:Insert(X,I)end end function D.GetBounds(d,X)local T=I[X]local Q=(d._center.X-d._corner1.X)*(T[1]-1)local v=(d._center.Y-d._corner1.Y)*(T[2]-1)local D=(d._center.Z-d._corner1.Z)*(T[3]-1)local M=Vector3.new(Q,v,D)local J=d._corner1+M local y=d._center+M return J,y end local function M(X,I,d,T)if not X then return d,T end if X._data then local Q=(X._coordinate-I).Magnitude if Q<T then d=X T=Q end end if X.ClassName==D.ClassName then local v=X._corner1 local D=X._corner2 local J=(v-D).Magnitude*.5 if(X._center-I).Magnitude<J+T then local v=Q(X._center,I)for Q=-1,6,1 do local D=(v+Q)%8+1 local J=X._children[D]d,T=M(J,I,d,T)end end end return d,T end function D.GetNearestNeighbor(d,X,I)assert(typeof(X)=="\086\101\099\116\111\114\051","\067\111\111\114\100\105\110\097\116\101\115 \109\117\115\116 \098\101 \086\101\099\116\111\114\051\033")I=I or math.huge local T,Q=M(d,X,nil,math.huge)if T and(X-T._coordinate).Magnitude<=I then return T._data,T._coordinate end end function D.Remove(I,X)assert(X,"\068\105\100 \110\111\116 \112\097\115\115 \100\097\116\097 \116\111 \114\101\109\111\118\101\033")local d=I._dataToLeaf[X]if not d then error("\079\098\106\101\099\116 \105\115 \110\111\116 \105\110 \111\099\116\114\101\101\033")end if d.ClassName==D.ClassName then d:_RemoveChild(d)else d._parent:_RemoveChild(d)end end function D.Contains(I,X)return I._dataToLeaf[X]~=nil end function D._RemoveChild(I,X)if not X then return end if X==I then I._dataToLeaf[I._data]=nil I._data=nil I._coordinate=nil end local d=I._data and 1 or 0 local T=I._data and I or nil for Q,v in pairs(I._children)do if v==X then if X._data then I._dataToLeaf[X._data]=nil end I._children[Q]=nil v:Destroy()else d=d+1 T=v end end if d<=0 and(I._parent and not I._data)then I._parent:_RemoveChild(I)end end function D.GetSize(X)local I=X._data and 1 or 0 for d,T in pairs(X._children)do if T.ClassName==X.ClassName then I=I+T:GetSize()else I=I+1 end end return I end return D
--[[
	OCTREE OBJECT
@public
	Octree.new()
--]]
local OCTANTS = {
    LeftBottomBack = 1,
    LeftBottomFront = 2,
    LeftTopBack = 3,
    LeftTopFront = 4,
    RightBottomBack = 5,
    RightBottomFront = 6,
    RightTopBack = 7,
    RightTopFront = 8
}
local BOUNDS_MAP = {
    [OCTANTS.LeftBottomBack] = {1, 1, 1},
    [OCTANTS.LeftBottomFront] = {1, 1, 2},
    [OCTANTS.LeftTopBack] = {1, 2, 1},
    [OCTANTS.LeftTopFront] = {1, 2, 2},
    [OCTANTS.RightBottomBack] = {2, 1, 1},
    [OCTANTS.RightBottomFront] = {2, 1, 2},
    [OCTANTS.RightTopBack] = {2, 2, 1},
    [OCTANTS.RightTopFront] = {2, 2, 2}
}
local OCTANT_MAP = {
    [1] = {
        [1] = {
            [1] = OCTANTS.LeftBottomBack,
            [2] = OCTANTS.LeftBottomFront
        },
        [2] = {
            [1] = OCTANTS.LeftTopBack,
            [2] = OCTANTS.LeftTopFront
        }
    },
    [2] = {
        [1] = {
            [1] = OCTANTS.RightBottomBack,
            [2] = OCTANTS.RightBottomFront
        },
        [2] = {
            [1] = OCTANTS.RightTopBack,
            [2] = OCTANTS.RightTopFront
        }
    }
}

local function isClose(a, b, eps)
    return (a - b).Magnitude <= eps
end

local function getOctant(center, pos)
    local x = (pos.X <= center.X) and 1 or 2
    local y = (pos.Y <= center.Y) and 1 or 2
    local z = (pos.Z <= center.Z) and 1 or 2
    return OCTANT_MAP[x][y][z]
end

local OctreeLeaf = require(script.OctreeLeaf)
local Octree = {}

Octree.__index = Octree
Octree.ClassName = script.Name

function Octree.new(corner_1, corner_2, epsilon, parent)
    assert(typeof(corner_1) == "Vector3", "Need vector3 corner_1")
    assert(typeof(corner_2) == "Vector3", "Need vector3 corner_2")
    local self = setmetatable({}, Octree)
    self._children = {}
    self._corner1 = corner_1
    self._corner2 = corner_2
    self._center = (corner_2 - corner_1) * .5 + corner_1 -- precompute center for efficiency
    self._parent = parent
    self._epsilon = epsilon or 1E-1
    self._dataToLeaf = {}
    if parent then
        assert(parent.ClassName == Octree.ClassName, "Tried to set invalid parent!")
        self._dataToLeaf = parent._dataToLeaf
    end
    return self
end

function Octree:Destroy()
    for k, v in pairs(self._children) do
        v:Destroy()
        self._children[k] = nil
    end
    self._children = nil
    self._corner1 = nil
    self._corner2 = nil
    self._center = nil
    self._parent = nil
    self._epsilon = nil
    self._data = nil
    self._coordinate = nil
    self._dataToLeaf = nil
end

function Octree:Insert(coordinate, data)
    assert(typeof(coordinate) == "Vector3", "Coordinates must be Vector3!")
    assert(data, "Tried to insert without data!")
    if (self._center - coordinate).Magnitude <= self._epsilon then
        self._data = data
        self._coordinate = coordinate
        self._dataToLeaf[data] = self
        return self
    end

    local octant = getOctant(self._center, coordinate)
    local child = self._children[octant]
    if not child then
        self._children[octant] = OctreeLeaf.new(coordinate, data, self)
        self._dataToLeaf[data] = self._children[octant]
    elseif child.ClassName == OctreeLeaf.ClassName then
        if (child._coordinate - coordinate).Magnitude <= self._epsilon then
            error("Tried to insert duplicate point in octree!")
        end
        local c1, c2 = self:GetBounds(octant)
        local newNode = Octree.new(c1, c2, self._epsilon, self)
        self._children[octant] = newNode
        newNode:Insert(child._coordinate, child._data)
        return newNode:Insert(coordinate, data)
    else
        return child:Insert(coordinate, data)
    end
end

function Octree:GetBounds(octant)
    local bounds = BOUNDS_MAP[octant]
    local xDif = (self._center.X - self._corner1.X) * (bounds[1] - 1)
    local yDif = (self._center.Y - self._corner1.Y) * (bounds[2] - 1)
    local zDif = (self._center.Z - self._corner1.Z) * (bounds[3] - 1)
    local diffVec = Vector3.new(xDif, yDif, zDif)
    local v1 = self._corner1 + diffVec
    local v2 = self._center + diffVec
    return v1, v2
end

local function recursiveNN(node, coord, best, dist)
    if not node then
        return best, dist
    end
    if node._data then
        local currDist = (node._coordinate - coord).Magnitude
        if currDist < dist then
            best = node
            dist = currDist
        end
    end
    if node.ClassName == Octree.ClassName then
        -- check if we should even go deeper (i.e. if it's even plausible the nearest neighbor is in bounding box)
        local c1 = node._corner1
        local c2 = node._corner2
        -- lazy check (quick spherical check)
        -- #TODO: Add tighter bounds check
        local radius = (c1 - c2).Magnitude * .5
        if (node._center - coord).Magnitude < radius + dist then
            local octant = getOctant(node._center, coord)
            -- we use the octant as a heuristic for the "most likely" next child
            -- #TODO: Add better heuristics
            for i = -1, 6 do
                local curr = (octant + i) % 8 + 1
                local child = node._children[curr]
                best, dist = recursiveNN(child, coord, best, dist)
            end
        end
    end
    return best, dist
end

function Octree:GetNearestNeighbor(coordinate, max_dist)
    assert(typeof(coordinate) == "Vector3", "Coordinates must be Vector3!")
    max_dist = max_dist or math.huge
    local best, dist = recursiveNN(self, coordinate, nil, math.huge)
    if best and (coordinate - best._coordinate).Magnitude <= max_dist then
        return best._data, best._coordinate
    end
end

function Octree:Remove(data)
    assert(data, "Did not pass data to remove!")
    local leaf = self._dataToLeaf[data]
    if not leaf then
        error("Object is not in octree!")
    end
    if leaf.ClassName == Octree.ClassName then
        leaf:_RemoveChild(leaf)
    else
        leaf._parent:_RemoveChild(leaf)
    end
end

function Octree:Contains(data)
    return self._dataToLeaf[data] ~= nil
end

function Octree:_RemoveChild(child)
    if not child then
        return
    end
    if child == self then
        -- special case for when the node itself contains the data
        self._dataToLeaf[self._data] = nil
        self._data = nil
        self._coordinate = nil
    end
    local childCount = self._data and 1 or 0
    local onlyChild = (self._data and self) or nil
    for k, v in pairs(self._children) do
        if v == child then
            if child._data then
                self._dataToLeaf[child._data] = nil
            end
            self._children[k] = nil
            v:Destroy()
        else
            childCount = childCount + 1
            onlyChild = v
        end
    end
    if childCount <= 0 and self._parent and not self._data then
        self._parent:_RemoveChild(self)
    end
end

function Octree:GetSize()
    local size = (self._data and 1) or 0
    for k, v in pairs(self._children) do
        if v.ClassName == self.ClassName then
            size = size + v:GetSize()
        else
            size = size + 1
        end
    end
    return size
end

return Octree
