args = {...}

rednet.open("right")
cx,cy,cz = gps.locate(5,false)
dx = args[1]
dy = args[2]
dz = args[3]
print("Current:"..cx..","..cy..","..cz)
print("Dest:"..dx..","..dy..","..dz)

distance = math.abs(dx-cx)
print("Distance:"..distance)

while(distance>0) do
  if(turtle.detect()) then
    turtle.up()
  end
  turtle.forward()
  cx,cy,cz = gps.locate(5,false)
  if math.abs(dx-cx) >= distance then
    turtle.turnRight()
  end

  distance = math.abs(dx-cx)
  print("Distance:"..distance)
end

turtle.turnRight()

distance = math.abs(dz-cz)
print("Distance:"..distance)

while(distance>0) do
  if(turtle.detect()) then
    turtle.up()
  end
  turtle.forward()
  cx,cy,cz = gps.locate(5,false)
  if math.abs(dz-cz) >= distance then
    turtle.turnRight()
    turtle.turnRight()
  end

  distance = math.abs(dz-cz)
  print("Distance:"..distance)
end

distance = math.abs(dy-cy)
print("Distance:"..distance)

while(distance>0) do
  dir = dy-cy
  if dir>0 then
    turtle.up()
  else
    turtle.down()
  end

  if turtle.detectUp() or turtle.detectDown() then
    print("Path blocked")
    return
  end

  cx,cy,cz = gps.locate(5,false)

  distance = math.abs(dy-cy)
  print("Distance:"..distance)
end

print("Arrived at destination")