# specific stuff for quad trees


mutable struct QuadTreeNode{T<:AbstractPoint2D} <: SpatialTreeNode
    id::Int64
    r::Float64
    midx::Float64
    midy::Float64
    is_empty::Bool
    is_divided::Bool
    point::T
    lxly::QuadTreeNode{T}
    lxhy::QuadTreeNode{T}
    hxly::QuadTreeNode{T}
    hxhy::QuadTreeNode{T}
    function QuadTreeNode{T}(r::Number, midx::Number, midy::Number) where T <:AbstractPoint2D
        n = new(0, r, midx, midy, true, false, T())
        n.lxly = n
        n.lxhy = n
        n.hxly = n
        n.hxhy = n
        n
    end
end

QuadTreeNode(r::Number, midx::Number, midy::Number, ::Type{T}) where T<:AbstractPoint2D =  QuadTreeNode{T}(r, midx, midy)
QuadTreeNode(::Type{T}) where T<:AbstractPoint2D = QuadTreeNode(0.5, 0.5, 0.5, T)
QuadTreeNode() = QuadTreeNode(Point2D);

mutable struct QuadTree{T<:AbstractPoint2D} <: SpatialTree
	head::QuadTreeNode{T}
	number_of_nodes_used::Int64
	nodes::Array{QuadTreeNode, 1}
    faststack::Array{QuadTreeNode{T}, 1}
    function QuadTree{T}(r::Number, midx::Number, midy::Number, n::Int64=100000) where T <:AbstractPoint2D
    	nodes = QuadTreeNode[QuadTreeNode(r, midx, midy, T) for i in 1:n]
        new(nodes[1], 1, nodes, [QuadTreeNode(T) for i in 1:10000])
    end
end

QuadTree(r::Number, midx::Number, midy::Number, ::Type{T};n=1000) where T<:AbstractPoint2D = QuadTree{T}(r, midx, midy, n)
QuadTree(::Type{T};n=10000) where T <: AbstractPoint2D = QuadTree(0.5, 0.5, 0.5, T; n=n)
QuadTree(n::Int64) = QuadTree(Point2D;n=n)
QuadTree() = QuadTree(Point2D)
Base.eltype(::QuadTree{T}) where T <: AbstractPoint2D = T

function initnode!(q::QuadTreeNode{T}, r::Number, midx::Number, midy::Number) where T <: AbstractPoint2D
    q.r = r
    q.midx = midx
    q.midy = midy
    q.is_empty = true
    q.is_divided = false
end

function divide!(h::QuadTree{T}, q::QuadTreeNode{T}) where T <: AbstractPoint2D
    # make sure we have enough nodes
    if length(h.nodes) - h.number_of_nodes_used < 4
    	new_size = length(h.nodes)+(length(h.nodes) >>> 1)
    	sizehint!(h.nodes, new_size)
    	for i in 1:(new_size-length(h.nodes))
    		push!(h.nodes, QuadTreeNode(T))
    	end
    end

    # populate new nodes
    q.lxly = h.nodes[h.number_of_nodes_used+1]
    q.lxhy = h.nodes[h.number_of_nodes_used+2]
    q.hxly = h.nodes[h.number_of_nodes_used+3]
    q.hxhy = h.nodes[h.number_of_nodes_used+4]

    # set new nodes properties (dimensions etc.)
    r2 = q.r/2
    initnode!(q.lxly, r2, q.midx-r2, q.midy-r2)
    initnode!(q.lxhy, r2, q.midx-r2, q.midy+r2)
    initnode!(q.hxly, r2, q.midx+r2, q.midy-r2)
    initnode!(q.hxhy, r2, q.midx+r2, q.midy+r2)

    # update tree and parent node
    h.number_of_nodes_used += 4
    q.is_divided = true
    if !q.is_empty
        # move point in parent node to child node
        sq = _getsubnode(q, q.point)
        sq.is_empty = false
        q.is_empty = true
        sq.point, q.point = q.point, sq.point
    end
    q
end

function _getsubnode(q::QuadTreeNode{T}, point::T) where T <: AbstractPoint2D
    x=getx(point)
    y=gety(point)
    if x<q.midx
        y<q.midy && return q.lxly
        return q.lxhy
    end
    y<q.midy && return q.hxly
    return q.hxhy
end

function map(t::QuadTree{T}, cond_data) where T <: AbstractPoint2D
    curr_stack_ix = 1
    t.faststack[1] = t.head
    @inbounds while curr_stack_ix > 0
        q = t.faststack[curr_stack_ix]
        curr_stack_ix -= 1
        if !stop_cond(q, cond_data) && q.is_divided
            curr_stack_ix += 1
            t.faststack[curr_stack_ix] = q.lxly
            curr_stack_ix += 1
            t.faststack[curr_stack_ix] = q.lxly
            curr_stack_ix += 1
            t.faststack[curr_stack_ix] = q.lxhy
            curr_stack_ix += 1
            t.faststack[curr_stack_ix] = q.lxhy
        end
    end
end
