CC: Goto

-- Don't use this program yet!
-- It's not ready for use!!

local tArgs = {...}
if #tArgs < 3 then
  write("Usage: ")
  write(shell.getRunningProgram())
  print(" <x> <y> <z>")
  return
end

if tonumber(tArgs[2]) < 0 then
  print("Too low y value!")
  return
end

--[[ Using relative pos ]]--

--local posX, posY, posZ = 0
--dir = 0

--[[ End ]]--

--[[ Using GPS ]]--

-- Direction:
-- +------ z ------+
-- |       0       |
-- x  1  turtle  3 |
-- |       2       |
-- +---------------+

local dir

rednet.open("right")
local posX, posY, posZ = gps.locate(.5)
local curX, curY, curZ = posX, posY, posZ
rednet.close("right")
if not posX then
  print("Could not locate position!")
  return
end

function checkGPS()
  rednet.open("right")
  curX, curY, curZ = gps.locate(.5)
  rednet.close("right")
end

function checkDir()
  if curX ~= posX then
    if curX < posX then
      dir = 3
    elseif curX > posX then
      dir = 1
    end
  elseif curZ ~= posZ then
    if curZ < posZ then
      dir = 2
    elseif curZ > posZ then
      dir = 0
    end
  end
end

function invertDir(dirVariable)
  if type(dirVariable) ~= "number" then
    error("Number expected, got "..type(dirVariable))
  end
  if dirVariable < 2 then
    return dirVariable + 2
  else
    return dirVariable - 2
  end
end

if not turtle.forward() then
  if not turtle.back() then
    turtle.turnRight()
    if not turtle.forward() then
      if not turtle.back() then
        turtle.turnLeft()
        print("No empty space found!")
        return
      else
        checkGPS()
        checkDir()
        while not turtle.forward() do
          if not turtle.dig() then
            turtle.attack()
          end
        end
      end
    else
      checkGPS()
      checkDir()
      for q = 1, 2 do
        turtle.turnRight()
      end
      while not turtle.forward() do
        if not turtle.dig() then
          turtle.attack()
        end
      end
      dir = invertDir(dir)
    end
  else
    checkGPS()
    checkDir()
    while not turtle.forward() do
      if not turtle.dig() then
        turtle.attack()
      end
    end
  end
else
  checkGPS()
  checkDir()
  for w = 1, 2 do
    turtle.turnRight()
  end
  while not turtle.forward() do
    if not turtle.dig() then
      turtle.attack()
    end
  end
  dir = invertDir(dir)
end

--[[ End ]]--

function goUp()
  while not turtle.up() do
    if not turtle.digUp() then
      if not turtle.attackUp() then
        sleep(.1)
      end
    end
  end
  posY = posY + 1
end
function goDown()
  while not turtle.down() do
    if not turtle.digDown() then
      if not turtle.attackDown() then
        sleep(.1)
      end
    end
  end
  posY = posY - 1
end
function left()
  if dir > 3-1 then
    dir = 0
  else
    dir = dir + 1
  end
  turtle.turnLeft()
end
function right()
  if dir < 0+1 then
    dir = 3
  else
    dir = dir - 1
  end
  turtle.turnRight()
end
function goForward()
  while not turtle.forward() do
    goUp()
  end
  if dir == 0 then
    posZ = posZ + 1
  elseif dir == 1 then
    posX = posX + 1
  elseif dir == 2 then
    posZ = posZ - 1
  elseif dir == 3 then
    posX = posX - 1
  else
    error("Undefined direction.")
  end
end
function goBack()
  while not turtle.back() do
    goUp()
  end
  if dir == 0 then
    posZ = posZ - 1
  elseif dir == 1 then
    posX = posX - 1
  elseif dir == 2 then
    posZ = posZ + 1
  elseif dir == 3 then
    posX = posX + 1
  else
    error("Undefined direction.")
  end
end
function faceZ()
  if dir ~= 0 then
    if dir == 1 then
      right()
    else
      repeat
        left()
      until dir == 0
    end
  end
end
function faceNZ()
  if dir ~= 2 then
    if dir == 3 then
      right()
    else
      repeat
        left()
      until dir == 2
    end
  end
end
function faceX()
  if dir ~= 1 then
    if dir == 2 then
      right()
    else
      repeat
        left()
      until dir == 1
    end
  end
end
function faceNX()
  if dir ~= 3 then
    if dir == 0 then
      right()
    else
      repeat
        left()
      until dir == 3
    end
  end
end
function goto(x,y,z)
  print("Heading to x"..x..", y"..y..", z"..z.."!")
  repeat
    if x < posX then
      faceNX()
    elseif x > posX then
      faceX()
    end
    while x ~= posX do
      goForward()
    end
    if z < posZ then
      faceNZ()
    elseif z > posZ then
      faceZ()
    end
    while z ~= posZ do
      goForward()
    end
    while y < posY do
      goDown()
    end
    while y > posY do
      goUp()
    end
  until x == posX and y == posY and z == posZ
end

local param1 = tonumber(tArgs[1])
local param2 = tonumber(tArgs[2])
local param3 = tonumber(tArgs[3])
--goto(param1,param2,param3)
faceZ()