-- goto.lua

--(( Variables ))--

x,y,z = 0,0,0
dir = 0
local init = false

--(( Methods ))--

function up()
  while not turtle.up() do
    if not turtle.attackUp() then
      print("ERROR: Failed to move up!")
      return false
    end
    sleep(1)
  end
  y = y + 1
  return true
end

function down()
  while not turlte.down() do
    if not turtle.attackDown() then
      print("ERROR: Failed to move down!")
      return false
    end
    sleep(1)
  end
  y = y - 1
  return true
end

function forward()
  while not turtle.forward() do
    if not turtle.attack() then
      print("ERROR: Failed to go forwards!")
      return false
    end
    sleep(1)
  end
  if dir == 0 then z = z + 1
  elseif dir == 1 then x = x - 1
  elseif dir == 2 then z = z - 1
  elseif dir == 3 then x = x + 1 end
  return true
end

function right()
  turtle.turnRight()
  dir = dir + 1
  if dir == 4 then dir = 0 end
end

function left()
  turtle.turnLeft()
  dir = dir - 1
  if dir == -1 then dir = 3 end
end

local function deltaDir(a,b)
  local delta = b - a
  if delta == 3 then return -1 end
  if delta == -3 then return 1 end
  return delta
end

function turn(towards)
  local delta = deltaDir(dir,towards)
  for x = 1,delta,1 do right() end
  for x = -1,delta,-1 do left() end
end

local function whereAmI()
  rednet.open("right")
  -- find start pos
  local nx,_,nz = gps.locate(.5)
  if not nx then error("ERROR: Failed to get my location!",0) end
  
  for count = 1,4 do
    if forward() then break
    else right() end
    if count == 4 then error("ERROR: Failed to get my direction!",0) end
  end
  
  x,y,z = gps.locate(.5)
  if not x then error("ERROR: Failed to get my location!",0) end
  
  if nx < x then dir = 3
  elseif nx > x then dir = 1
  elseif nz < z then dir = 0
  elseif nz > z then dir = 2
  else
    error("ERROR: Invalid direction!",0)
  end
  
  rednet.close("right")
  init = true
end

function goto(nx,ny,nz)
  if not init then whereAmI() end
  
  while x~=nx or y~=ny or z~=nz do
    print("Heading to x"..nx.." y"..ny.." z"..nz.."!")
    
    -- X axis
    if nx > x then turn(3) end
    if nx < x then turn(1) end
    repeat until nx == x or not forward()
    
    -- Z axis
    if nz > z then turn(0) end
    if nz < z then turn(2) end
    repeat until nz == z or not forward()
    
    -- Up
    while ny > y do up() end
    
    -- Down
    while ny < y do down() end
  end
end

-- EOF