blockMap

-- Data collection functions
local scan = {}

scan.clean = function(rawScan, posX, posY, posZ)
    local myPos = vector.new(0,0,0)
    if type(posX) == "number" then
        myPos.x = posX
    end
    if type(posY) == "number" then
        myPos.y = posY
    end
    if type(posZ) == "number" then
        myPos.z = posZ
    end
    local cleanedScan = {}
    for i = 1,#rawScan do
        local name = rawScan[i].name or ""
        if name:find(":") then name = name:sub(name:find(":")+1,#name) end
        rawScan[i].name = name
        if rawScan[i].name == "air" then
            rawScan[i].name = ""
        elseif rawScan[i].name == "concrete" or rawScan[i].name == "concrete_powder" or rawScan[i].name == "wool" or rawScan[i].name == "planks" or rawScan[i].name == "log" then
            if rawScan[i].state and rawScan[i].state.color then
                rawScan[i].name = rawScan[i].name .. "_" .. rawScan[i].state.color
            elseif rawScan[i].state and rawScan[i].state.variant then
                rawScan[i].name = rawScan[i].name .. "_" .. rawScan[i].state.variant
            end
        end
        local pos = vector.new(rawScan[i].x,rawScan[i].y,rawScan[i].z)
        pos = pos + myPos
        cleanedScan[pos.y] = cleanedScan[pos.y] or {}
        cleanedScan[pos.y][pos.x] = cleanedScan[pos.y][pos.x] or {}
        cleanedScan[pos.y][pos.x][pos.z] = rawScan[i].name
    end
    return cleanedScan
end

scan.scanner = function(posX, posY, posZ)
    local rawScan = {}
    local scanner = peripheral.find("plethora:scanner")
    if not scanner then local modular = peripheral.find("manipulator") or peripheral.find("neuralInterface") end
    if modular and modular.hasModule("plethora:scanner") then scanner = modular end
    if not scanner then
        error("'.scanner()' requires Plethora Block Scanner. Try using '.inspect()' if unavailable.")
    else
        rawScan = scanner.scan()
    end
    local cleanedScan = scan.clean(rawScan, posX, posY, posZ)
    return cleanedScan
end

-- Use a turtle to scan blocks around it
-- Returns absolute position if turtle co-ordinates are given, or relative if not
-- Requires direction turtle is facing
-- 'inspectAll' boolean determines whether the turtle will spin to get the remaining 3 blocks.
scan.inspect = function(facing, inspectAll, posX, posY, posZ)
    local rawScan = {}
    if not turtle then
        error("Mode 'inspect' requires device to be a turtle.")
    elseif facing ~= "north" and facing ~= "east" and facing ~= "south" and facing ~= "west" then
        error("Mode 'inspect' requires direction turtle is currently facing (e.g. 'north')")
    elseif type(inspectAll) ~= "nil" and type(inspectAll) ~= "boolean" then
        error("Mode 'inspect' Argument #2 expects boolean, for whether to spin and get all surrounding blocks.")
    else
        local blockPos = {
            ["north"] = vector.new( 0, 0,-1),
            ["east"]  = vector.new( 1, 0, 0),
            ["south"] = vector.new( 0, 0, 1),
            ["west"]  = vector.new(-1, 0, 0),
        }

        rawScan[1] = {
            x = 0,
            y = 0,
            z = 0,
            name = "turtle"
        }

        rawScan[2] = {turtle.inspectUp()}
        if not rawScan[2][1] then rawScan[2][2] = {} end
        rawScan[2] = rawScan[2][2]
        rawScan[2].x = 0
        rawScan[2].y = 1
        rawScan[2].z = 0

        rawScan[3] = {turtle.inspectDown()}
        if not rawScan[3][1] then rawScan[3][2] = {} end
        rawScan[3] = rawScan[3][2]
        rawScan[3].x = 0
        rawScan[3].y = -1
        rawScan[3].z = 0

        rawScan[4] = {turtle.inspect()}
        if not rawScan[4][1] then rawScan[4][2] = {} end
        rawScan[4] = rawScan[4][2]
        rawScan[4].x = blockPos[facing].x
        rawScan[4].y = blockPos[facing].y
        rawScan[4].z = blockPos[facing].z
        if inspectAll == true then
            for i = 5,7 do
                turtle.turnRight()
                if facing == "north" then facing = "east"
                elseif facing == "east" then facing = "south"
                elseif facing == "south" then facing = "west"
                elseif facing == "west" then facing = "north" end
                rawScan[i] = {turtle.inspect()}
                if not rawScan[i][1] then rawScan[i][2] = {} end
                rawScan[i] = rawScan[i][2]
                rawScan[i].x = blockPos[facing].x
                rawScan[i].y = blockPos[facing].y
                rawScan[i].z = blockPos[facing].z
            end
            turtle.turnRight()
        end
    end
    local cleanedScan = scan.clean(rawScan, posX, posY, posZ)
    return cleanedScan
end

-- Map utility functions
local map = {}

-- Convert raw map data into two tables: Blocks and air cluster
map.compress = function(mapData)
    -- Sort the co-ordinates by size
    local coordinates = {}
    for y, xzTbl in pairs(mapData) do
        for x, zTbl in pairs(mapData[y]) do
            for z, _ in pairs(mapData[y][x]) do
                table.insert(coordinates, { y = y, x = x, z = z })
            end
        end
    end
    table.sort(coordinates, function(a, b)
        if a.y ~= b.y then
            return a.y < b.y
        elseif a.x ~= b.x then
            return a.x < b.x
        else
            return a.z < b.z
        end
    end)

    -- Split the sorted map into maps for air and blocks
    local airClusterMap = {}
    local discoveredMap = {}
    local currentAirCluster = nil
    local last = {y = nil, x = nil, z = nil}

    for _, coord in ipairs(coordinates) do
        local y, x, z = coord.y, coord.x, coord.z
        if currentAirCluster and y == last.y and x == last.x and z == last.z + 1 and mapData[y][x][z] == "" then
            currentAirCluster.e = z
        elseif currentAirCluster then
            airClusterMap[last.y] = airClusterMap[last.y] or {}
            airClusterMap[last.y][last.x] = airClusterMap[last.y][last.x] or {}
            currentAirCluster.e = last.z
            table.insert(airClusterMap[last.y][last.x], currentAirCluster)
            currentAirCluster = nil
        end
        if not currentAirCluster and mapData[y][x][z] == "" then
            currentAirCluster = { s = z }
        elseif mapData[y][x][z] ~= "" then
            discoveredMap[y] = discoveredMap[y] or {}
            discoveredMap[y][x] = discoveredMap[y][x] or {}
            discoveredMap[y][x][z] = mapData[y][x][z]
        end
        last = {y = y, x = x, z = z}
    end
    if currentAirCluster then
        airClusterMap[last.y] = airClusterMap[last.y] or {}
        airClusterMap[last.y][last.x] = airClusterMap[last.y][last.x] or {}
        currentAirCluster.e = last.z
        table.insert(airClusterMap[last.y][last.x], currentAirCluster)
    end
    local deflatedMap = {airClusterMap, discoveredMap}
    return deflatedMap
end

-- Convert a compressed map back into the raw format
map.decompress = function(mapData)
    local inflatedMap = {}
    if #mapData == 2 then
        local airClusterMap, discoveredMap = unpack(mapData)
        if airClusterMap and discoveredMap then
            for y, clusterMapX in pairs(airClusterMap) do
                inflatedMap[y] = inflatedMap[y] or {}
                for x, clusterList in pairs(airClusterMap[y]) do
                    inflatedMap[y][x] = inflatedMap[y][x] or {}
                    for clusters, cluster in pairs(airClusterMap[y][x]) do
                        for z = cluster.s, cluster.e do
                            inflatedMap[y][x][z] = ""
                        end
                    end
                end
            end
            for y, discoveredXZ in pairs(discoveredMap) do
                inflatedMap[y] = inflatedMap[y] or {}
                for x, discoveredZ in pairs(discoveredMap[y]) do
                    inflatedMap[y][x] = inflatedMap[y][x] or {}
                    for z, blockName in pairs(discoveredMap[y][x]) do
                        inflatedMap[y][x][z] = blockName
                    end
                end
            end
        end
    end
    return inflatedMap
end

-- Combine two maps, overlaps treat old map as outdated
map.update = function(oldMap, newMap)
    local updatedMap = nil
    if type(oldMap) == "table" and type(newMap) == "table" then
        for y,xzTbl in pairs(newMap) do
            if oldMap[y] and type(newMap[y]) == "table" then
                for x,row in pairs(newMap[y]) do
                    if oldMap[y][x] and type(newMap[y][x]) == "table" then
                        for z,block in pairs(newMap[y][x]) do
                            oldMap[y][x][z] = block
                        end
                    else
                        oldMap[y][x] = row
                    end
                end
            else
                oldMap[y] = xzTbl
            end
        end
        updatedMap = oldMap
    end
    return updatedMap
end

-- Return a table of map data trimmed for easy map drawing
map.draw = function(mapTable, tileset, radius, midY, midX, midZ)
    if not mapTable then mapTable = {} end
    if not tileset then tileset = {} end
    if radius and type(radius) == "number" then
        radiusX = math.abs(radius)
        radiusZ = math.abs(radius)
    end
    local maxX,maxZ = term.current().getSize()
    if not radiusX or (radiusX*2)+1 > maxX then
        radiusX = (maxX / 2)
        if radiusX > math.floor(radiusX) then radiusX = math.floor(radiusX)
        else radiusX = radiusX - 1 end
    end
    if not radiusZ or (radiusZ*2)+1 > maxZ then
        radiusZ = (maxZ / 2)
        if radiusZ > math.floor(radiusZ) then radiusZ = math.floor(radiusZ)
        else radiusZ = radiusZ - 1 end
    end
    local mid = vector.new(0,0,0)
    if midY and type(midY) == "number" then
        mid.y = midY
    end
    if midX and type(midX) == "number" then
        mid.x = midX
    end
    if midZ and type(midZ) == "number" then
        mid.z = midZ
    end
    local drawnMap = {}
    for x = 1, 1+(2*radiusX) do
        if not drawnMap[x] then drawnMap[x] = {} end
        for z = 1, 1+(2*radiusZ) do
            if not drawnMap[x][z] then drawnMap[x][z] = {} end
            drawnMap[x][z].x = mid.x + x - 1 - radiusX
            drawnMap[x][z].y = mid.y
            drawnMap[x][z].z = mid.z + z - 1 - radiusZ
            if mapTable[mid.y] and mapTable[mid.y][drawnMap[x][z].x] and mapTable[mid.y][drawnMap[x][z].x][drawnMap[x][z].z] then
                drawnMap[x][z].name = mapTable[mid.y][drawnMap[x][z].x][drawnMap[x][z].z]
            else
                drawnMap[x][z].name = "unknown"
            end
            if drawnMap[x][z].name == "" then drawnMap[x][z].name = "air" end
            if tileset[drawnMap[x][z].name] then
                if tileset[drawnMap[x][z].name].tile then
                    drawnMap[x][z].tile = tileset[drawnMap[x][z].name].tile
                end
                if tileset[drawnMap[x][z].name].fg then
                    drawnMap[x][z].fg = tileset[drawnMap[x][z].name].fg
                end
                if tileset[drawnMap[x][z].name].bg then
                    drawnMap[x][z].bg = tileset[drawnMap[x][z].name].bg
                end
            end
        end
    end
    return drawnMap
end

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 mappedBlocks[v.y] and mappedBlocks[v.y][v.x] and mappedBlocks[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 mappedBlocks[goal.y] and mappedBlocks[goal.y][goal.x] and mappedBlocks[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 mappedBlocks[path[j].y] or not mappedBlocks[path[j].y][path[j].x] or not mappedBlocks[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

-- Find a route between two points using a given map
mappedBlocks = {}
map.pathfind = function(mapData, startX, startY, startZ, endX, endY, endZ, timeout)
    mappedBlocks = mapData
    local start = {x = tonumber(startX), y = tonumber(startY), z = tonumber(startZ)}
    local goal = {x = tonumber(endX), y = tonumber(endY), z = tonumber(endZ)}
    local timer = tonumber(timeout) or 5
    path = {start}
    deadNodes = {}
    local hasPath = findPath(start,goal,timer)
    if hasPath then
        findPath(goal,start,timer)
        path = removeDetours(path)
        path = smoothPath(path)
        foundPath = ""
        for i = 2,#path do
            local x = path[i].x - path[i-1].x
            local y = path[i].y - path[i-1].y
            local z = path[i].z - path[i-1].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

map.query = function(mapData,blockName)
    local qResult = {}
    for y,_ in pairs(mapData) do
        for x,_ in pairs(mapData[y]) do
            for z,block in pairs(mapData[y][x]) do
                if block == blockName then
                    table.insert(qResult, {x = x, y = y, z = z})
                end
            end
        end
    end
    return qResult
end

return {
inspect = scan.inspect,
scanner = scan.scanner,
compress = map.compress,
decompress = map.decompress,
update = map.update,
create = map.draw,
path = map.pathfind,
query = map.query,
}