local turtlePos, direction
local startPos, startDir

local function getGPSPosition()
    local x, y, z = gps.locate()
    if not x then print("Error: GPS not available") return nil end
    return {x = math.floor(x), y = math.floor(y), z = math.floor(z)}
end

local function negative(vec)
    return {x = -vec.x, y = -vec.y, z = -vec.z}
end

local function add(vec1, vec2)
    return {x = vec1.x + vec2.x, y = vec1.y + vec2.y, z = vec1.z + vec2.z}
end
local function sub(vec1, vec2)
    return add(vec1, negative(vec2))
end

local function multiply(s, vec)
    return {x = s * vec.x, y = s * vec.y, z = s * vec.z}
end

local function getSign(number)
    return number / math.abs(number)
end

local function isEqual(vec1, vec2)
    return vec1.x == vec2.x and vec1.y == vec2.y and vec1.z == vec2.z
end


-- === Basic Movement ===

local function moveForward()
    if turtle.forward() then
        turtlePos = add(turtlePos, direction)
        return true
    end
    return false
end
local function moveUp()
    if turtle.up() then
        turtlePos.y = turtlePos.y + 1
        return true
    end
    return false
end

local function moveDown()
    if turtle.down() then
        turtlePos.y = turtlePos.y - 1
        return true
    end
    return false
end

local function forceMoveForward()
    while not moveForward() do turtle.dig() end
end

local function forceMoveUp()
    while not moveUp() do turtle.digUp() end
end

local function forceMoveDown()
    while not moveDown() do turtle.digDown() end
end

-- === Basic Turning ===

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

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

    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

local function getDirection()
    local pos1 = getGPSPosition()
    if not pos1 then error("GPS failure, cannot get direction") end

    for i = 1, 4 do
        if turtle.forward() then
            local pos2 = getGPSPosition()
            if not pos2 then error("GPS failure after move") end
            -- turtlePos = pos2
            -- Richtung berechnen
            direction = sub(pos2, pos1)

            -- zurück zur ursprünglichen Position
            turnInDirection("around")
            forceMoveForward()

            -- Sicherheitsprüfung
            local posBack = getGPSPosition()
            if not isEqual(pos1, posBack) then
                error("Warnung: Position hat sich beim Rückweg verschoben!")
                turtlePos = posBack -- explizit wieder auf GPS setzen
            else
                turtlePos = pos1
            end

            turnInDirection("around")
            return
        else
            turtle.turnLeft()
        end
    end

    error("Unable to move forward in any direction for direction detection")
end


-- === Positioning ===

local function goToPos(coord)
    local deltaX = coord.x - turtlePos.x
    local deltaY = coord.y - turtlePos.y
    local deltaZ = coord.z - turtlePos.z

    local dirX = startDir.x
    local dirZ = startDir.z

    local function moveToEqualX()
        while not (deltaX == 0) do
            turnInDirection(getTurnDirection(direction, {x = getSign(deltaX), y = 0, z = 0}))
            forceMoveForward()
            deltaX = coord.x - turtlePos.x
        end
    end

    local function moveToEqualZ()
        while not (deltaZ == 0) do
            turnInDirection(getTurnDirection(direction, {x = 0, y = 0, z = getSign(deltaZ)}))
            forceMoveForward()
            deltaZ = coord.z - turtlePos.z
        end
    end

    local function moveToEqualY()
        while not (deltaY == 0) do
            if deltaY > 0 then
                forceMoveUp()
            elseif deltaY < 0 then
                forceMoveDown()
            end
            deltaY = coord.y - turtlePos.y
        end
    end

    
    if dirX == 0 then
        moveToEqualZ()
        moveToEqualX()
    elseif dirZ == 0 then
        moveToEqualX()
        moveToEqualZ()
    end

    if not (deltaY == 0) then
        moveToEqualY()
        deltaX = coord.x - turtlePos.x
        deltaZ = coord.z - turtlePos.z
    end
end

------------------------- Startpos adapten
local function goToStartOfTree()
    startTreePos = add(startPos, multiply(7, startDir))
    goToPos(startTreePos)
    turnInDirection(getTurnDirection(direction, startDir))
end

-- === Tasks ===

local function isSapling(item)
    if not item or not item.name then return false end

    local saplingSuffix = "pale_oak_sapling"
    return item.name:match(":.*" .. saplingSuffix .. "$") ~= nil
end

local function isBoneMeal(item)
    if not item or not item.name then return false end
    return item.name == "minecraft:bone_meal"
end

local function isLog(item)
    if not item or not item.name then return false end

    local logSuffix = "_log"
    return item.name:match(":.*" .. logSuffix .. "$") ~= nil
end

local function getBoneMealCount()
    local num = 0
    for i = 1, 16 do
        local item = turtle.getItemDetail(i)
        
        if isBoneMeal(item) then
            num = num + turtle.getItemCount(i)
        end
    end
    return num
end
local function getSaplingCount()
    local num = 0
    for i = 1, 16 do
        local item = turtle.getItemDetail(i)
        
        if isSapling(item) then
            num = num + turtle.getItemCount(i)
        end
    end
    return num
end

local function selectSaplingSlot()
    for i = 1, 16 do
        local item = turtle.getItemDetail(i)
        
        if isSapling(item) then
            turtle.select(i)
            return true
        end
    end
    return false
end

local function selectBoneMealSlot()
    for i = 1, 16 do
        local item = turtle.getItemDetail(i)
        
        if isBoneMeal(item) then
            turtle.select(i)
            return true
        end
    end
    return false
end

local function plantSapling()
    if not selectSaplingSlot() then return false end
    if not turtle.place() then return false end
    return true
end

local function plantAllSaplings()
    local function plantSaplingDown()
        if not selectSaplingSlot() then return false end
        if not turtle.placeDown() then
            suc, item = turtle.inspectDown()
            if suc and isSapling(item) then return true end
            return false
        end
        return true
    end
    goToStartOfTree()
    forceMoveUp()

    suc, item = turtle.inspect()
    if suc and isLog(item) then
        goToStartOfTree()
        return false
    end

    local returnValue = true

    forceMoveForward()
    returnValue = returnValue and plantSaplingDown()
    forceMoveForward()
    returnValue = returnValue and plantSaplingDown()

    turnInDirection("right")
    forceMoveForward()
    turnInDirection("right")

    returnValue = returnValue and plantSaplingDown()
    forceMoveForward()
    returnValue = returnValue and plantSaplingDown()

    goToStartOfTree()
    return returnValue
end

local function fellTillLeaves_GetTurnDir(cnt)
    local sucUp, itemUp = turtle.inspectUp()
    local sucFor, itemFor = turtle.inspect()

    local topTurnDir = "left"

    while (not (sucUp or itemUp.name == "minecraft:pale_oak_leaves")) and cnt < 20 do
        if sucFor and isLog(itemFor.name) then
            turtle.dig()
        end
        forceMoveUp()
        sucUp, itemUp = turtle.inspectUp()
        sucFor, itemFor = turtle.inspect()
        cnt = cnt + 1
    end
    
    if (not (sucFor or itemFor.name == "minecraft:pale_oak_leaves") and cnt < 20) then
        topTurnDir = "right"
        forceMoveForward()
        sucUp, itemUp = turtle.inspectUp()
        while (not (sucUp or itemUp.name == "minecraft:pale_oak_leaves")) and cnt < 20 do
            forceMoveUp()
            sucUp, itemUp = turtle.inspectUp()
            cnt = cnt + 1
        end
    end
    return topTurnDir
end

local function fellAtPos(startFellPos)
    goToPos(startFellPos)
    turnInDirection(getTurnDirection(direction, startDir))

    local cnt = turtlePos.y - startFellPos.y
    local topTurnDir = fellTillLeaves_GetTurnDir(cnt)
    cnt = turtlePos.y - startFellPos.y

    turnInDirection("right")
    forceMoveForward()
    turnInDirection(topTurnDir)

    topTurnDir = fellTillLeaves_GetTurnDir(cnt)
    
    if topTurnDir == "right" then
        turnInDirection("around")
    end

    while turtlePos.y > startFellPos.y do
        local sucFor, itemFor = turtle.inspect()
        if sucFor and isLog(itemFor.name) then
            turtle.dig()
        end
        forceMoveDown()
    end

end

local function fellTree()
    local forwDir = startDir
    turnInDirection("left")
    local leftDir = direction

    local diffRight = negative(multiply(2, leftDir))
    local diffFor = multiply(2, forwDir)

    local startFellPos1 = add(negative(forwDir),add(turtlePos, multiply(2, leftDir)))
    local startFellPos2 = add(startFellPos1, diffRight)
    local startFellPos3 = add(startFellPos2, diffRight)

    local startFellPos4 = add(startFellPos3, diffFor)
    local startFellPos5 = add(startFellPos2, diffFor)
    local startFellPos6 = add(startFellPos1, diffFor)

    local startFellPos7 = add(startFellPos6, diffFor)
    local startFellPos8 = add(startFellPos5, diffFor)
    local startFellPos9 = add(startFellPos4, diffFor)

    fellAtPos(startFellPos1)
    fellAtPos(startFellPos2)
    fellAtPos(startFellPos3)
    fellAtPos(startFellPos4)
    fellAtPos(startFellPos5)
    fellAtPos(startFellPos6)
    fellAtPos(startFellPos7)
    fellAtPos(startFellPos8)
    fellAtPos(startFellPos9)

    goToStartOfTree()

end

-- === Inventory Management ===

local function compactInventory()
    for i = 1, 16 do
        local itemI = turtle.getItemDetail(i)
        if itemI and itemI.count < 64 then
            for j = i + 1, 16 do
                local itemJ = turtle.getItemDetail(j)
                if itemJ and itemJ.name == itemI.name then
                    turtle.select(j)
                    turtle.transferTo(i)
                    
                    -- Aktuellen Slot updaten
                    itemI = turtle.getItemDetail(i)
                    if not itemI or itemI.count == 64 then
                        break
                    end
                end
            end
        end
    end
    turtle.select(1) -- zum Standard zurückkehren
end

-- Keep saplings
local function dumpInventoryKeepSaplings(n)
    local success, chest = turtle.inspect()
    while not (success and chest.name == "minecraft:chest") do
        print("No chest found. Pls place one in front.")
        print("Press enter to continue.")
        read()
        success, chest = turtle.inspect()
    end

    local saplingsKept = 0

    for slot = 1, 16 do
        local item = turtle.getItemDetail(slot)
        if item then
            turtle.select(slot)
            if isSapling(item) and saplingsKept < n then
                local keep = math.min(item.count, n - saplingsKept)
                local drop = item.count - keep
                if drop > 0 then
                    turtle.drop(drop)
                end
                saplingsKept = saplingsKept + keep
            elseif not isBoneMeal(item) then
                turtle.drop()
            end
        end
    end

    turtle.select(1)
end

local function turtleNeedsFuel()
    local fuelLevel = turtle.getFuelLevel()

    if fuelLevel < 1000 then
        if not isEqual(startPos, turtlePos) then
            diffX = math.abs(startPos.x - turtlePos.x)
            diffY = math.abs(startPos.y - turtlePos.y)
            diffZ = math.abs(startPos.z - turtlePos.z)

            if not (fuelLevel < diffX + diffY + diffZ +5) then
                goToPos(startPos)
            end
        end
        print("Turtle needs more fuel")
        return true
    end
    return false
end

-- gets specified item in given amount (min(available, requested)) from chest
local function suckSpecificItem(itemName, count, strDir)
    local chest = peripheral.wrap(strDir)
    if not chest or not chest.list then
        return false
    end

    local chestItems = chest.list()
    local foundSlots = {}
    local totalAvailable = 0

    -- 1. Scanne Kiste nach dem gewünschten Item
    for slot, item in pairs(chestItems) do
        if item.name == itemName then
            table.insert(foundSlots, {slot = slot, count = item.count})
            totalAvailable = totalAvailable + item.count
        end
    end
    count = math.min(totalAvailable, count)
    if count == 0 then
        return false
    end

    -- 2. Finde freien Slot in der Turtle für gecachtes Item
    local cachedItemSlot = nil
    for i = 1, 16 do
        if turtle.getItemCount(i) == 0 then
            cachedItemSlot = i
            break
        end
    end

    if not cachedItemSlot then
        print("Kein freier Slot zum Zwischenspeichern des Kisten-Items.")
        return false
    end

    turtle.select(cachedItemSlot)
    local cachedItem = nil

    local list = chest.list()
    if list[1] and list[1].name ~= itemName then
        if strDir == "front" then
            if turtle.suck(64) then
                cachedItem = turtle.getItemDetail(cachedItemSlot)
            end
        elseif strDir == "top" then
            if turtle.suckUp(64) then
                cachedItem = turtle.getItemDetail(cachedItemSlot)
            end
        elseif strDir == "bottom" then
            if turtle.suckDown(64) then
                cachedItem = turtle.getItemDetail(cachedItemSlot)
            end
        end
    end

    -- 3. Erstes gewünschtes Item in Slot 1 schieben
    local nextSlotIndex = 1
    if not chest.getItemDetail(1) then
        local nextSource = foundSlots[nextSlotIndex]
        chest.pushItems(strDir, nextSource.slot, nextSource.count, 1)
        nextSlotIndex = nextSlotIndex + 1
    elseif (chest.getItemDetail(1) and chest.getItemDetail(1).name == itemName) then
        nextSlotIndex = nextSlotIndex + 1
    end

    -- 4. Gecachtes Item zurückgeben
    if cachedItem then
        turtle.select(cachedItemSlot)
        if strDir == "front" then
            turtle.drop() -- Kiste verteilt es automatisch auf einen freien Slot ≠ 1
        elseif strDir == "top" then
            turtle.dropUp() -- Kiste verteilt es automatisch auf einen freien Slot ≠ 1
        elseif strDir == "bottom" then
            turtle.dropDown() -- Kiste verteilt es automatisch auf einen freien Slot ≠ 1
        end
    end

    -- 5. Zielitems absaugen
    local collected = 0
    turtle.select(cachedItemSlot) -- Wiederverwenden für Einsammeln
    while collected < count do
        local remaining = count - collected
        local slot1Item = chest.getItemDetail(1)
        local pullCount = math.min(slot1Item.count, remaining)
        
        if strDir == "front" then
            if turtle.suck(pullCount) then
                collected = collected + pullCount
            end
        elseif strDir == "top" then
            if turtle.suckUp(pullCount) then
                collected = collected + pullCount
            end
        elseif strDir == "bottom" then
            if turtle.suckDown(pullCount) then
                collected = collected + pullCount
            end
        end

        

        if collected < count and nextSlotIndex <= #foundSlots then
            local nextSource = foundSlots[nextSlotIndex]
            chest.pushItems(strDir, nextSource.slot, nextSource.count, 1)
            nextSlotIndex = nextSlotIndex + 1
        end
    end

    return true
end


local function processAndStockInventory()
    goToPos(startPos)
    turnInDirection(getTurnDirection(direction, negative(startDir)))
    dumpInventoryKeepSaplings(64)
    if getSaplingCount() < 8 then
        suckSpecificItem("minecraft:pale_oak_sapling", 64 - getSaplingCount(), "bottom")
        bFirst = true
        while getSaplingCount() < 8 do
            if bFirst then print("Waiting for new Saplings!") bFirst = false end
            if not suckSpecificItem("minecraft:pale_oak_sapling", 64 - getSaplingCount(), "bottom") then sleep(5) end
        end
    end
    if getBoneMealCount() < 64 then
        suckSpecificItem("minecraft:bone_meal", 64 - getBoneMealCount(), "top")
        bFirst = true
        while getBoneMealCount() < 10 do
            if bFirst then print("Waiting for new Bone Meal!") bFirst = false end
            if not suckSpecificItem("minecraft:bone_meal", 64 - getBoneMealCount(), "top") then sleep(5) end
        end
    end
    compactInventory()
    goToStartOfTree()
end


-- === Initialization ===

local function destroyTreeBlocks()
    local suc, item = turtle.inspect()
    if not suc then
        return
    end

    local treeBlocks = {"minecraft:pale_oak_leaves", "minecraft:pale_oak_log"}

    cnt = 1
    while cnt <= 2 do
        if item.name == treeBlocks[cnt] then
            turtle.dig()
            return
        end
        cnt = cnt + 1
    end
end

local function saveStartingParameters()
    local parameterList = {pos = turtlePos, dir = direction}

    local file = fs.open("paleOakStartingParameters.txt", "w")
    file.write(textutils.serialize(parameterList))
    file.close()
end

local function loadStartingParameters()
    if not fs.exists("paleOakStartingParameters.txt") then return false end

    local file = fs.open("paleOakStartingParameters.txt", "r")
    local data = textutils.unserialize(file.readAll())

    file.close()

    startPos = data.pos
    startDir = data.dir
    return true
end

local function initProgram()
    turtlePos = getGPSPosition()
    if not turtlePos then error("GPS failure, cannot run the program") end
    
    destroyTreeBlocks()
    getDirection()

    if not loadStartingParameters() then
        startPos = turtlePos
        startDir = direction
        saveStartingParameters()
    end
end



-- === Main ===

local function main()
    bSaplingPlanted = true

    while turtle.getFuelLevel() < 5 do
        print("Turtle needs more fuel")
        return
    end

    initProgram()
    if turtleNeedsFuel() then return end

    if (getBoneMealCount() == 0) or (getSaplingCount() < 4) then
        processAndStockInventory()
    end

    goToStartOfTree()
    plantAllSaplings()
    while true do
        if turtleNeedsFuel() then return end
        if bSaplingPlanted then
            suc, item = turtle.inspect()
            if suc and isSapling(item) then
                if selectBoneMealSlot() then
                    turtle.place()
                else
                    processAndStockInventory()
                end
            else
                if suc and isLog(item) then
                    fellTree()
                    bSaplingPlanted = false
                end

                slotCount = turtle.getItemCount(16)
                if slotCount > 0 then
                    processAndStockInventory()
                end
            end
        else
            if not plantAllSaplings() then
                processAndStockInventory()
                if not plantAllSaplings() then
                    goToPos(startPos)
                    print("Error: Saplings cannot be planted!")
                    return
                end
            end
            bSaplingPlanted = true
        end
    end

end

main()