mappedPathfinding

local isDead = function(node)
    for _,dead in pairs(deadNodes) do
        if node.y == dead.y and node.x == dead.x and node.z == dead.z then
            return true
        end
    end
    return false
end

local isVisited = function(node)
    for _,existing in pairs(path) do
        if node.y == existing.y and node.x == existing.x and node.z == existing.z then
            return true
        end
    end
    return false
end

local heuristic = function(node, goal)
    return math.abs(goal.x - node.x) + math.abs(goal.y - node.y) + math.abs(goal.z - node.z)
end

local findNext = function(current, goal, previous)
    local pNeighbours = {
        {x = current.x + 1, y = current.y, z = current.z},
        {x = current.x - 1, y = current.y, z = current.z},
        {x = current.x, y = current.y, z = current.z + 1},
        {x = current.x, y = current.y, z = current.z - 1},
        {x = current.x, y = current.y + 1, z = current.z},
        {x = current.x, y = current.y - 1, z = current.z},
    }
    local neighbours = {}

    for k, v in pairs(pNeighbours) do
        if mapData[v.y] and mapData[v.y][v.x] and mapData[v.y][v.x][v.z] == "" then
            local cost = 1 -- Assuming each movement has a cost of 1 for simplicity
            table.insert(neighbours, {x = v.x, y = v.y, z = v.z, cost = cost})
        end
    end

    table.sort(neighbours, function(a, b)
        return (a.cost + heuristic(a, goal)) < (b.cost + heuristic(b, goal))
    end)

    if #neighbours == 0 then
        table.insert(deadNodes, current)
    end

    return neighbours
end

local isValid = function(start,goal)
    if mapData[goal.y] and mapData[goal.y][goal.x] and mapData[goal.y][goal.x][goal.z] == "" then
        if #findNext(goal,start) > 0 and #findNext(start,goal) > 0 then
            --textutils.slowPrint(textutils.serialise(findNext(start,goal)))
            return true
        end
    end
    return false
end

local findPath = function(start, goal, timeout)
    local time = os.clock() + timeout
    local found = false
    local exploredNodesCount = 0
    local maxStaleIterations = 10 -- Adjust this value based on your needs

    if isValid(start, goal) then
        while not isDead(start) and not found and os.clock() < time do
            if (path[#path].x == goal.x and path[#path].y == goal.y and path[#path].z == goal.z) then
                found = true
                break
            end

            os.queueEvent("yield")
            os.pullEvent("yield")
            local next = findNext(path[#path], goal, path[#path - 1])

            if #next == 0 then
                table.remove(path, #path)
            else
                local newNode = false

                for k, v in pairs(next) do
                    if not isVisited(v) then
                        newNode = true
                        table.insert(path, v)
                        exploredNodesCount = exploredNodesCount + 1
                        break
                    end
                end

                if not newNode then
                    table.insert(deadNodes, path[#path])
                    table.remove(path, #path)
                end
            end

            -- Check for staleness and break out of the loop if no progress is made
            if exploredNodesCount > 0 then
                exploredNodesCount = 0
            else
                maxStaleIterations = maxStaleIterations - 1
                if maxStaleIterations <= 0 then
                    break
                end
            end
        end
    end

    return found
end

local function isNeighbor(node1, node2)
    local dx = math.abs(node2.x - node1.x)
    local dy = math.abs(node2.y - node1.y)
    local dz = math.abs(node2.z - node1.z)

    return (dx == 1 and dy == 0 and dz == 0) or
           (dx == 0 and dy == 1 and dz == 0) or
           (dx == 0 and dy == 0 and dz == 1)
end

local function smoothPath(path)
    local smoothPath = {path[1]}
    local i = 1

    while i < #path - 1 do
        local j = i + 1
        local straightLine = isNeighbor(path[i], path[j])

        while j <= #path do
            if not mapData[path[j].y] or not mapData[path[j].y][path[j].x] or not mapData[path[j].y][path[j].x][path[j].z] == "" then
                break
            end

            local canSee = true

            for k = i + 1, j - 1 do
                if not isDead(path[k]) then
                    canSee = false
                    break
                end
            end

            if canSee then
                if not straightLine or isNeighbor(path[j - 1], path[j]) then
                    smoothPath[#smoothPath + 1] = path[j]
                end
                i = j - 1
                break
            end

            j = j + 1
            straightLine = isNeighbor(path[i], path[j])
        end

        i = i + 1
    end

    -- Ensure the smoothed path includes the end point
    smoothPath[#smoothPath + 1] = path[#path]

    return smoothPath
end

local function removeDetours(path)
    local i = 1

    while i <= #path - 2 do
        local j = #path

        while j > i + 1 do
            if isNeighbor(path[i], path[j]) then
                -- Nodes i and j are neighbors, remove the intermediate nodes
                for k = i + 1, j - 1 do
                    table.remove(path, i + 1)
                end
                j = i + 2
            else
                j = j - 1
            end
        end

        i = i + 1
    end

    return path
end

local find = function(mappedBlocks,myX,myY,myZ,gX,gY,gZ,timeout)
    mapData = mappedBlocks
    local start = {x = tonumber(myX), y = tonumber(myY), z = tonumber(myZ)}
    local goal = {x = tonumber(gX), y = tonumber(gY), z = tonumber(gZ)}
    local timer = tonumber(timeout) or 5
    path = {start}
    deadNodes = {}
    local hasPath = findPath(start,goal,timer)
    if hasPath then
        existingPath = path
        path = {goal}
        findPath(goal,start,timer)
        path = removeDetours(path)
        path = smoothPath(path)
        existingPath = removeDetours(existingPath)
        existingPath = smoothPath(existingPath)
        if #existingPath < #path then
            path = existingPath
            loopStart = 2
            loopEnd = #path
            loopD = 1
        else
            loopStart = #path - 1
            loopEnd = 1
            loopD = -1
        end
        foundPath = ""
        for i = loopStart,loopEnd,loopD do
            local x = path[i].x - path[i-loopD].x
            local y = path[i].y - path[i-loopD].y
            local z = path[i].z - path[i-loopD].z
            if x > 0 then foundPath = foundPath.."e"
            elseif x < 0 then foundPath = foundPath.."w"
            elseif z > 0 then foundPath = foundPath.."s"
            elseif z < 0 then foundPath = foundPath.."n"
            elseif y > 0 then foundPath = foundPath.."u"
            elseif y < 0 then foundPath = foundPath.."d" end
        end
        return foundPath
    end
    return hasPath
end

return {find = find}