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

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

for k,v in pairs(face) do
  if tArgs[4] == v then
    dirDest = k
  elseif tonumber(tArgs[4]) ~= nil then
    dirDest = tArgs[4]
  else
    print("Direction Destination Required")
  end
end

function correctUsage()
  if #tArgs ~= 3 then
    print("Correct usage: goTo <X> <Y> <Z> <Dir>")
  end
local a,b,c = gps.locate(5,true)
  if a==nil then
    print("GPS Array out of range")
  end
end

function direction()
  xPos, yPos, zPos = gps.locate()
  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(fDir)
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 right()
  turtle.turnRight()
  fDir = fDir + 1
  if fDir > 3 then fDir = 0 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()
  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

function goToDir()
  while dirDest ~= fDir do
    turtle.turnRight()
  end
end

correctUsage()
direction()
goToX()
goToZ()
goToY()
direction()
goToDir()