StorageTurtle

-- === Turtle Navigation for Warehouse System ===
-- Assumes turtle starts on init disk, facing toward the system
-- Uses GPS for position, init.txt for orientation info

-- === Utility ===
local function readInitData()
    local initDiskPath
    for _, side in ipairs(peripheral.getNames()) do
        if peripheral.getType(side) == "drive" then
            local disk = peripheral.wrap(side)
            if disk.getDiskLabel() == "Turtle Initializing" then
                initDiskPath = disk.getMountPath()
                break
            end
        end
    end
    if not initDiskPath then error("Init disk not found") end

    local initFile = fs.open(fs.combine(initDiskPath, "init.txt"), "r")
    local orientation = initFile.readLine():match("orientation=(%a+)")
    local direction = tonumber(initFile.readLine():match("direction=(%-?%d+)"))
    local modulo = tonumber(initFile.readLine():match("modulo=(%d+)"))
    initFile.close()

    return orientation, direction, modulo
end

local function getDirectionVectors(orientation, direction)
    if orientation == "x" then
        return {
            forward = {x = direction, y = 0, z = 0},
            right   = {x = 0, y = 0, z = direction},
            up      = {x = 0, y = 1, z = 0}
        }
    elseif orientation == "z" then
        return {
            forward = {x = 0, y = 0, z = direction},
            right   = {x = -direction, y = 0, z = 0},
            up      = {x = 0, y = 1, z = 0}
        }
    else
        error("Invalid orientation")
    end
end

local function getStartPosition()
    local x, y, z = gps.locate()
    if not x then error("GPS not available") end
    return {x = math.floor(x), y = math.floor(y), z = math.floor(z)}
end

local function add(pos, delta)
    return {
        x = pos.x + delta.x,
        y = pos.y + delta.y,
        z = pos.z + delta.z
    }
end

-- === Facing and Rotation Management ===
local facingVec
local function updateFacingVec(vec)
    facingVec = vec
end

local function turnAndUpdate(dir)
    if dir == "left" then
        turtle.turnLeft()
        facingVec = {x = facingVec.z, y = 0, z = -facingVec.x}
    elseif dir == "right" then
        turtle.turnRight()
        facingVec = {x = -facingVec.z, y = 0, z = facingVec.x}
    elseif dir == "around" then
        turtle.turnLeft()
        turtle.turnLeft()
        facingVec = {x = -facingVec.x, y = 0, z = -facingVec.z}
    end
end

local function vectorsEqual(a, b)
    return a.x == b.x and a.z == b.z
end

local function getTurnDirection(from, to)
    if vectorsEqual(from, to) then return nil end
    local left  = {x = from.z, y = 0, z = -from.x}
    local right = {x = -from.z, y = 0, z = from.x}
    local back  = {x = -from.x, y = 0, z = -from.z}
    if vectorsEqual(to, left) then
        return "left"
    elseif vectorsEqual(to, right) then
        return "right"
    elseif vectorsEqual(to, back) then
        return "around"
    else
        error("Invalid target direction")
    end
end

-- === Movement ===
local function move(directionVec)
    if directionVec.y > 0 then
        while not turtle.up() do sleep(0.5) end
    elseif directionVec.y < 0 then
        while not turtle.down() do sleep(0.5) end
    elseif directionVec.x ~= 0 or directionVec.z ~= 0 then
        local turnDir = getTurnDirection(facingVec, directionVec)
        if turnDir then
            turnAndUpdate(turnDir)
        end
        while not turtle.forward() do sleep(0.5) end
    end
end

local function moveMultiple(vec, amount)
    local step = (amount > 0 and 1 or -1)
    for _ = 1, math.abs(amount) do
        move({x = vec.x * step, y = vec.y * step, z = vec.z * step})
    end
end

-- === Initialization ===
local orientation, direction, modulo = readInitData()
local dirs = getDirectionVectors(orientation, direction)
local relativeDirs = {
    forward = dirs.forward,
    right   = dirs.right,
    up      = dirs.up,
    down    = {x = -dirs.up.x, y = -dirs.up.y, z = -dirs.up.z},
    left    = {x = -dirs.right.x, y = 0, z = -dirs.right.z},
    back    = {x = -dirs.forward.x, y = 0, z = -dirs.forward.z},
}
updateFacingVec(relativeDirs.forward)

local startPos = getStartPosition()
local finishPos = add(startPos, {
    x = 2 * dirs.forward.x - 5 * dirs.right.x,
    y = -1,
    z = 2 * dirs.forward.z - 5 * dirs.right.z
})

-- === Core Functions ===
local function initialMoveToFinish()
    moveMultiple(dirs.forward, 3)
    moveMultiple(relativeDirs.left, 5)
    moveMultiple(relativeDirs.down, 1)
    moveMultiple(relativeDirs.back, 1)
    turnAndUpdate("left")
end

local function acceptTask()
    print("[TASK] Accepting task...")
    turnAndUpdate("left")
    move(facingVec)
    moveMultiple(relativeDirs.up, 1)
    move(facingVec)
    -- TODO: actual task logic
end

local function travelToTask()
    print("[STATE] Traveling to task...")
    -- TODO
end

local function performTask()
    print("[STATE] Performing task...")
    -- TODO
end

local function returnToFinish()
    print("[STATE] Returning to finish block...")
    -- TODO
end

-- === Queue Handling ===
local function isTurtleBlock(data)
    return data and type(data.name) == "string" and data.name:match("computercraft:turtle")
end

local function enterQueue()
    print("[QUEUE] Entering queue...")
    moveMultiple(relativeDirs.left, 6)
    local level = 0
    while true do
        turnAndUpdate("left")
        local success, data = turtle.inspect()
        turnAndUpdate("right")
        if not (success and isTurtleBlock(data)) then
            turnAndUpdate("left")
            move(facingVec)
            if level % 2 == 0 then
                turnAndUpdate("left")
            else
                turnAndUpdate("right")
            end
            break
        else
            moveMultiple(relativeDirs.up, 1)
            level = level + 1
        end
    end

    if level > 0 and level % 2 == 0 then
        while turtle.forward() do
            local belowSuccess, belowData = turtle.inspectDown()
            if not (belowSuccess and isTurtleBlock(belowData)) then
                moveMultiple(relativeDirs.down, 1)
                turnAndUpdate("around")
                break
            elseif turtle.detect() then
                break
            end
        end
    end
end

local function queueForTask()
    print("[QUEUE] Advancing in queue...")
    local lastWasDescent = false
    while true do
        local downSuccess, downData = turtle.inspectDown()
        if downSuccess and downData.name == "computercraft:disk_drive" then
            break
        elseif not turtle.detect() then
            while not turtle.forward() do sleep(0.5) end
            lastWasDescent = false
        else
            local frontSuccess, frontData = turtle.inspect()
            if frontSuccess and not isTurtleBlock(frontData) then
                local downAgain, downInfo = turtle.inspectDown()
                if not lastWasDescent and not (downAgain and isTurtleBlock(downInfo)) then
                    moveMultiple(relativeDirs.down, 1)
                    turnAndUpdate("around")
                    lastWasDescent = true
                else
                    sleep(0.5)
                end
            else
                sleep(0.5)
            end
        end
    end
    print("Leaving Queue")
end

-- === Runtime ===
initialMoveToFinish()

while true do
    enterQueue()
    queueForTask()
    acceptTask()
    travelToTask()
    performTask()
    returnToFinish()
    break           -- zum Testen
end