[TURTLE] Goto

local tArgs = { ... }

-- 0: north 1: east 2: south 3: west
local direction = 0

local currentX, currentY, currentZ
local targetX, targetY, targetZ

if #tArgs < 3 then
    print("Usage: goto <x> <y> <z>")
    return
end
targetX = tArgs[1]
targetY = tArgs[2]
targetZ = tArgs[3]

function determineLocation()
    currentX, currentY, currentZ = gps.locate()
    print("Current Position: "..currentX..":"..currentY..":"..currentZ)
end

function determineDirection()
    determineLocation()
    turtle.forward()
    newX, newY, newZ = gps.locate()
    turtle.back()

    deltaX = newX - currentX
    deltaZ = newZ - currentZ

    if deltaX <= -1 then
        direction = 3
    elseif deltaX >= 1 then
        direction = 1
    elseif deltaZ <= -1 then
        direction = 0
    elseif deltaZ >= 1 then
        direction = 2
    end
    print("Delta: "..deltaX..":"..deltaY..":"..deltaZ)
    print("Facing: "..direction)
end

function face(newDir)
    if direction < newDir then
        for i=1, newDir - direction do
            turtle.turnRight()
        end
        direction = newDir
    else
        for i=1, direction - newDir do
            turtle.turnLeft()
        end
        direction = newDir
    end
end

function move()
    print("Navigating to: "..targetX..", "..targetY..", "..targetZ)
    deltaX = math.abs(currentX - targetX)
    deltaY = math.abs(currentY - targetY)
    deltaZ = math.abs(currentZ - targetZ)
    totalFuel = deltaX + deltaY + deltaZ
    print("Needed: "..totalFuel.." got "..turtle.getFuelLevel())
    if turtle.getFuelLevel() < totalFuel then
        print("Not enough fuel to complete the task")
        return
    end
    print(deltaX..":"..deltaY..":"..deltaZ)
    if tonumber(targetX) < tonumber(currentX) then
        face(3)
    else
        face(1)
    end
    for i=1,deltaX do
        if not turtle.forward() then
            turtle.dig()
        end
    end
    -- Go to the z coord
    if tonumber(targetZ) < tonumber(currentZ) then
        face(0)
    else
        face(2)
    end
    for i=1,deltaZ do
        if not turtle.forward() then
            turtle.dig()
        end
    end
    -- Go to the y coord
    for i=1,deltaY do
        if tonumber(currentY) > tonumber(targetY) then
            if turtle.down() then
                turtle.digDown()
            end
        else
            if turtle.up() then
                turtle.digUp()
            end
        end
    end
end
determineDirection()
move()