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

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 = "_propagule"
    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 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 fellBranch(bDown)
    -- if turtle goes down the tree
    bDown = bDown or false

    local suc, item = turtle.inspect()
    if not(suc and isLog(item)) then
        return false
    end
    if bDown then
        forceMoveDown()
        fellBranch()
        forceMoveUp()
    end

    branchDir = direction


    forceMoveForward()

    local function advanceInBranch()
        forceMoveForward()
        local suc, item = turtle.inspectUp()
        if suc and isLog(item) then
            forceMoveUp()
            advanceInBranch()
            forceMoveDown()
        end
        turnInDirection(getTurnDirection(direction, negative(branchDir)))
        forceMoveForward()
    end

    advanceInBranch()
    turnInDirection(getTurnDirection(direction, negative(branchDir)))
    forceMoveForward()
    turnInDirection(getTurnDirection(direction, branchDir))
    return true
end

local function fellTree()
    forceMoveForward()

    cnt = 0
    suc, item = turtle.inspectUp()
    while not (suc and isLog(item)) do
        forceMoveUp()
        suc, item = turtle.inspectUp()
        cnt = cnt + 1
        if cnt >= 25 then
            goToStartOfTree()
            return false
        end
    end

    forceMoveUp()
    startLogY = turtlePos.y

    fellBranch()

    suc, item = turtle.inspectUp()
    while suc and isLog(item) do
        forceMoveUp()
        fellBranch()
        suc, item = turtle.inspectUp()
    end

    endLogY = turtlePos.y

    turnInDirection("right")
    fellBranch(true)
    while not (turtlePos.y == startLogY) do
        forceMoveDown()
        fellBranch(true)
    end
    turnInDirection("right")
    fellBranch()
    while not (turtlePos.y == endLogY) do
        forceMoveUp()
        fellBranch()
    end
    turnInDirection("right")
    fellBranch(true)
    while not (turtlePos.y == startLogY) do
        forceMoveDown()
        fellBranch(true)
    end
    goToStartOfTree()

    return true
end



-- === Initialization ===

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

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

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

    local file = fs.open("startingParameters.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
    
    getDirection()

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



-- === Main ===

local function main()
    initProgram()
    goToStartOfTree()
    fellTree()
end

main()