pathfind.lua

local sides = require "sides"
local vect = require "bh/libvec"
local os = require "os"
local path = {}

function path.vtos(rel)
    if rel.x > 0 then
        return sides.posx
    elseif rel.x < 0 then
        return sides.negx
    elseif rel.y > 0 then
        return sides.posz
    elseif rel.y < 0 then
        return sides.negz
    elseif rel.z > 0 then
        return sides.posy
    elseif rel.z < 0 then
        return sides.negy
    end
end

function path.stov(side)
    if side == sides.posx then
        return vect.new(1,0,0)
    elseif side == sides.negx then
        return vect.new(-1,0,0)
    elseif side == sides.posz then
        return vect.new(0,1,0)
    elseif side == sides.negz then
        return vect.new(0,-1,0)
    elseif side == sides.posy then
        return vect.new(0,0,1)
    elseif side == sides.negy then
        return vect.new(0,0,-1)
    end
end

function path.vtoi(p)
    return (p.x+32)+((p.y+32)*64)+((p.z+32)*64*64)
end

function path.itov(idx)
    local z = math.floor(idx/(64*64))
    local y = math.floor((idx-(64*64*z))/(64))
    local x = math.floor(idx-(64*64*z)-(64*y))
    return vect.new(x-32,y-32,z-32)
end

function path.unpackNode(num)
    local x = (num & (127<<15)) >> 15
    local y = (num & (127<<22)) >> 22
    local z = (num & (127<<29)) >> 29
    local n = vect.new(x-32,y-32,z-32)
    n.parent = num & (7<<0)
    n.facing = (num & (7<<3)) >> 3
    n.dist = (num & (255<<6)) >> 6
    n.explored = (num & (1<<14)) >> 14
    n.cost = (num & (1023<<36)) >> 36
    return n
end

function path.packNode(n)
    if n.parent > 7 or n.facing > 7 or n.cost > 1023 or n.dist > 255 or n.explored > 1 then
        return false
    elseif n.parent < 0 or n.facing < 0 or n.cost < 0 or n.dist < 0 or n.explored < 0 then
        return false
    end
    return (n.parent<<0) | (n.facing<<3) |  (n.dist<<6) | (n.explored << 14) | ((n.x+32)<<15) | ((n.y+32)<<22) | ((n.z+32)<<29) | (n.cost<<36)
end

local _path = {
    getCost = function(self, p1, p2)
        local cost = self.weights[1]
        local side = path.vtos(p2-p1)
        if side ~= sides.top and side ~= sides.bottom and side ~= p1.facing then
            cost = cost + self.weights[2]
        end
        return cost
    end,

    getNeighbors = function(self, point)
        local ns = {}
        local ps = {{-1, 0, 0}, {0, -1, 0}, {0, 0, -1}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}
        for k,v in pairs(ps) do
            local nbr = point+vect.new(v[1],v[2],v[3])
            if math.abs(nbr.x) <= 32 and math.abs(nbr.y) <= 32 and math.abs(nbr.z) <= 32 then
                if self.nodes[path.vtoi(nbr)] == nil then
                    ns[#ns+1] = nbr
                end
            end
        end
        return ns
    end,

    queueInsert = function(self, n1)
        for qk,qv in pairs(self.queue) do
            local n1Cost = (n1.cost * self.weights[3]) + (n1.dist * self.weights[4])
            local n2 = path.unpackNode(self.nodes[qv])
            local n2Cost = (n2.cost * self.weights[3]) + (n2.dist * self.weights[4])
            if n1Cost < n2Cost then
                table.insert(self.queue, qk, path.vtoi(n1))
                return
            end
        end
        self.queue[#self.queue+1] = path.vtoi(n1)
    end,

    reversePath = function(self, node)
        local p = tostring(node.facing)
        local parent = path.unpackNode(self.nodes[path.vtoi(node+stov(node.parent))])
        while not (tostring(parent) == tostring(vect.new(0,0,0))) do
            p = parent.facing..p
            parent = path.unpackNode(self.nodes[path.vtoi(parent+stov(parent.parent))])
        end
        return p
    end,

    dist = function(self,p1,p2)
        local rel = p1 - p2
        if self.noy then
            return math.floor(math.abs(rel.x)+math.abs(rel.y))
        else
            return math.floor(math.abs(rel.x)+math.abs(rel.y)+math.abs(rel.z))
        end
    end,

    check = function(self, rel)
        --out of range
        if math.abs(rel.x) > 32 or math.abs(rel.y) > 32 or rel.z > 31 or rel.z < -32 then
            return false
        end

        --geolyzer scan to make sure we can reach the block
        local data = geo.scan(rel.x, rel.y)
        if data[rel.z+33] > 0 then
            return false
        end

        --avoid flying too high
        local idx = rel.z+33-7
        if idx < 1 then
            idx = 1
        end
        while idx < rel.z+33 do
            if data[idx] > 0 then
                return true
            end
            idx = idx + 1
        end
        return false
    end,

    getPath = function(self, p1, p2)
        if self:dist(p1,p2) < 1 then
            return ""
        end

        self.goal = p2 - p1
        self.start = p1
        if math.abs(self.goal.x) > 32 or math.abs(self.goal.y) > 32 or ((self.noy == false) and (math.abs(self.goal.z) > 32)) then
            print(self.goal)
            return false
        end

        local start = vect.new(0,0,0)
        self.nodes = {}
        self.queue = {}
        start.parent = 7
        start.facing = 7
        start.cost = 0
        start.dist = self:dist(start,self.goal)
        start.explored = 0
        self.nodes[path.vtoi(start)] = path.packNode(start)
        self.queue[1] = path.vtoi(start)

        self.pathing = true
        while self.pathing do
            os.sleep(0)
            if #self.queue < 1 then
                self.pathing = false
                return false
            end
            local node = path.unpackNode(self.nodes[table.remove(self.queue, 1)])
            if node.explored == 0 then
                if self:check(node) or ((node-start):length() == 0) then
                    local ns = self:getNeighbors(node)
                    node.explored = 1
                    self.nodes[path.vtoi(node)] = path.packNode(node)
                    for nk,nv in pairs(ns) do
                        nv.dist = self:dist(nv,self.goal)
                        nv.cost = math.floor(self:getCost(node,nv) + (self.weights[5]*node.cost))
                        nv.parent = path.vtos(node-nv)
                        nv.facing = path.vtos(nv-node)
                        nv.explored = 0
                        if nv.dist == 0 and self:check(nv) then
                            self.pathing = false
                            local p = self:reversePath(nv)
                            if self.clear then
                                self.nodes = nil
                                self.queue = nil
                                self.goal = nil
                            end
                            return p, vect.new(nv.x+p1.x,nv.y+p1.y,nv.z+p1.z)
                        end
                        self.nodes[path.vtoi(nv)] = path.packNode(nv)
                        self:queueInsert(nv)
                    end
                end
            end
        end
    end
}

function path.new(weights, noy, clear)
    local p = {}
    p.pathing = false
    p.weights = weights or {1,1,0.5,1,1}
    p.noy = noy or false
    p.clear = clear or true
	setmetatable(p, {__index = _path})
	return p
end

return path