Goto

local hx = 771
local hy = 65
local hz = -1345

local tArgs = {...}

if #tArgs ~= 3 then
  print('Usage: goto <x> <y> <z>')
  return
end

function location()
  local x, y, z = gps.locate(4)
  if not x then
    error('Could not find location!')
  else
    return x, y, z
  end
end

function distance(x, y, z)
  local _x, _y, _z = location()
  local dx = x - _x
  local dy = y - _y
  local dz = z - _z
  return dx, dy, dz
end

local dir = {
  n = 1,
  e = 2,
  s = 3,
  w = 4
}

function move(d)
  if d == 'up' then
    if not turtle.up() then
      turtle.digUp()
      turtle.up()
    end
  elseif d == 'down' then
    if not turtle.down() then
      turtle.digDown()
      turtle.down()
    end
  elseif d == 'foward' then
    if not turtle.forward() then
      turtle.dig()
      turtle.forward()
    end
  elseif d == 'back' then
    if not turtle.back() then
      turtle.turnLeft()
      turtle.turnLeft()
      turtle.dig()
      turtle.turnLeft()
      turtle.turnLeft()
      turtle.back()
    end
  end
end

function direction()
  local x, y, z = location()
  move('forward')
  local _x, _y, _z = location()
  move('back')
  if _x > x then
    return dir.e
  elseif x > _x then
    return dir.w
  elseif _z > z then
    return dir.s
  elseif z > _z then
    return dir.n
  else
    error('Unknown direction.')
  end
end

function turn(d)
  local turns = direction() - d
  if turns == 0 then
    return
  elseif turns > 0 then
    for i = 1, turns do
      turtle.turnLeft()
    end
  else
    for i = 1, math.abs(turns) do
      turtle.turnRight()
    end
  end
end

function goto(x, y, z)
  local dx, dy, dz = distance(x, y, z)
  if dx > 0 then
    turn(dir.e)
  elseif dx < 0  then
    turn(dir.w)
  end

  for i = 1, math.abs(dx) do
    move('forward')
  end

  if dz > 0 then
    turn(dir.s)
  elseif dz < 0 then
    turn(dir.n)
  end

  for i = 1, math.abs(dz) do
    move('forward')
  end

  local f = 'up'
  if dy < 0 then
    f = 'down'
  end

  for i = 1, math.abs(dy) do
    move(f)
  end
end

print('Going to target!')
local tx = tonumber(tArgs[1])
local ty = tonumber(tArgs[2])
local tz = tonumber(tArgs[3])
local cx, cy, cz = location()
print('Climbing to cruise height')
goto(cx, 255, cz)
print('Transversing to target')
goto(tx, 255, tz)
print('Descending')
goto(tx, ty, tz)
print('Arrived. Press any key when done')
read()
print('Climbing to cruise height')
goto(tx, 255, tz)
print('Returning home')
goto(hx, 255, hz)
print('Landing')
goto(hx, hy, hz)
print('Done!')