Turtle GoTo

local tArgs = {...}
xDest, yDest, zDest = tonumber(tArgs[1]), tonumber(tArgs[2]), tonumber(tArgs[3])

direct = {}
direct[0] = "south"
direct[1] = "west"
direct[2] = "north"
direct[3] = "east"

function finalDir()
  destDir = nil
  if tArgs[4] then
    for i, v in pairs(direct) do
      if tArgs[4] == v then
        destDir = i
      end
    end
    while fDir ~= destDir do
      left()
    end
  end
end

function correctUsage()
  if #tArgs < 3 or #tArgs > 4 then
    print("Correct usage: goTo <X> <Y> <Z> (<ending direction>)")
    print("NOTE: Requires GPS API")
  end
end

function direction()
  xPos, yPos, zPos = gps.locate()
  if not xPos or not yPos or not zPos then
    print("ERROR: GPS API REQUIRED")
    os.exit(goTo)
  end
  turtle.forward()
  xPos2, yPos2, zPos2 = gps.locate()
  turtle.back()
  if xPos ~= xPos2 then
    if xPos > xPos2 then
      fDir = 1
    else
      fDir = 3
    end
  else
    if zPos > zPos2 then
      fDir = 2
    else
      fDir = 0
    end
  end
  print("Initial direction: " .. tonumber(fDir) .. "::" .. direct[tonumber(fDir)])
  if tArgs[4] then
    print("Final direction: " .. tonumber(destDir) .. "::" .. direct[tonumber(destDir)])
  else
    print("Final direction: Undefined")
  end
end

function distanceFormula()
  xMath = math.abs(xPos - xDest)
  yMath = math.abs(yPos - yDest)
  zMath = math.abs(zPos - zDest)
  hyp = math.sqrt(xMath^2 + yMath^2 + zMath^2)

  print("Distance: " .. hyp .. " blocks")
end

function forward()
  turtle.forward()
  if fDir == 0 then zPos = zPos + 1 end
  if fDir == 2 then zPos = zPos - 1 end
  if fDir == 1 then xPos = xPos - 1 end
  if fDir == 3 then xPos = xPos + 1 end
end

function left()
  turtle.turnLeft()
  fDir = fDir - 1
  if fDir < 0 then fDir = 3 end
end

function goToX()
  if xDest < xPos then
    while fDir ~= 1 do
      left()
    end
  elseif xDest > xPos then
    while fDir ~= 3 do
      left()
    end
  end
  while xPos ~= xDest do
    turtle.dig()
    forward()
    print(xPos)
  end
end

function goToZ()
  if zDest < zPos then
    while fDir ~= 2 do
      left()
    end
  elseif zDest > zPos then
    while fDir ~= 0 do
      left()
    end
  end
  while zPos ~= zDest do
    turtle.dig()
    forward()
  end
end

function goToY()
  if yDest < yPos then
    while yDest ~= yPos do
      turtle.digDown()
      turtle.down()
      yPos = yPos - 1
    end
  elseif yDest > yPos then
    while yDest ~= yPos do
      turtle.digUp()
      turtle.up()
      yPos = yPos + 1
    end
  end
end

correctUsage()
direction()
distanceFormula()
write("Traveling... ")
goToX()
goToZ()
goToY()
print("Complete")
finalDir()