Npc pathfindg

local start = Vector3.new(345, 69, 339)
local step = 3
local size = Vector3.new(3, 3, 3)
local spawned = {}

local function key(p)
    return p.X..","..p.Y..","..p.Z
end

local function snap(p)
    return Vector3.new(math.floor(p.X/step + 0.5)*step, p.Y, math.floor(p.Z/step + 0.5)*step)
end

local function getRandomGoal()
    local players = PlayerService.getPlayers()
    if #players == 0 then return start end
    local p = players[math.random(1,#players)]
    local e = p:getEntity()
    if not e then return start end
    local pos = e:getPosition()
    if not pos then return start end
    local gx = ((pos.X+1)//3*3)
    local gy = ((pos.Y+1)//3*3)
    local gz = ((pos.Z+1)//3*3)
    return Vector3.new(gx,gy,gz), pos
end

local goal, rawGoalPos = getRandomGoal()

local function free(p)
    return BlockService.getBlockAt(p) == nil
end

local function hasBlockBelow(p)
    local below = Vector3.new(p.X, p.Y-step, p.Z)
    return BlockService.getBlockAt(below) ~= nil
end

local function place(p, color)
    if not hasBlockBelow(p) then return false end
    local k = key(p)
    if spawned[k] then return false end
    spawned[k] = true
    local part = PartService.createPart(color or ItemType.CLAY_WHITE, p)
    part:setSize(size)
    part:setAnchored(true)
    part:setCollidable(false)
    return true
end

local function equal(a,b)
    return a.X==b.X and a.Y==b.Y and a.Z==b.Z
end

local function getNeighbors(p)
    local n = {}
    local dirs = {Vector3.new(step,0,0), Vector3.new(-step,0,0), Vector3.new(0,0,step), Vector3.new(0,0,-step)}
    for i = #dirs, 2, -1 do
        local j = math.random(1,i)
        dirs[i], dirs[j] = dirs[j], dirs[i]
    end
    for _, d in ipairs(dirs) do
        for dy=-step,step,step do
            local np = Vector3.new(p.X+d.X, p.Y+dy, p.Z+d.Z)
            if free(np) and hasBlockBelow(np) then
                table.insert(n, np)
                break
            end
        end
    end
    return n
end

local function buildPath(s,g)
    local queue = {s}
    local came = {[key(s)]=s}
    local found,nodes,maxNodes = false,0,4000

    while #queue>0 do
        local p = table.remove(queue,1)
        if equal(p,g) then found=true break end
        nodes=nodes+1
        if nodes>maxNodes then
            place(rawGoalPos, ItemType.CLAY_RED)
            print("UNREACHABLE:",rawGoalPos)
            return
        end
        for _, np in ipairs(getNeighbors(p)) do
            local k = key(np)
            if not came[k] then
                came[k]=p
                table.insert(queue,np)
            end
        end
    end

    if not found then
        place(rawGoalPos, ItemType.CLAY_RED)
        print("UNREACHABLE:",rawGoalPos)
        return
    end

    local path={}
    local cur=g
    while true do
        table.insert(path,cur)
        if equal(cur,s) then break end
        cur = came[key(cur)]
    end

    for i=#path,1,-1 do
        place(path[i])
    end
end

buildPath(start,goal)