1 files changed, 47 insertions, 3 deletions
diff --git a/nav.lua b/nav.lua
index 354897a..62b8457 100644
--- a/nav.lua
+++ b/nav.lua
@@ -31,13 +31,57 @@ local function moveX(x1, x2, o1)
 	return o2
 end
 
-local function travel(x1, y1, z1, o1, x2, y2, z2, o2, safe_y)
+local function moveZ(z1, z2, o1)
+	if z1 == z2 then
+		return o1
+	end
+
+	local amount = z2 - z1
+	local o2 = amount > 0 and 2 or 4
+	turnTo(o1, o2)
+
+	for ii = 1, math.abs(amount) do
+		turtle.forward()
+	end
+
+	return o2
+end
+
+local function moveY(y1, y2)
+	if y1 == y2 then
+		return nil
+	end
+
+	local amount = y2 - y1
+	for ii = 1, math.abs(amount) do
+		if y2 > y1 then
+			turtle.up()
+		else
+			turtle.down()
+		end
+	end
+end
+
+local function travel(x1, y1, z1, o1, x2, y2, z2, o2, safe_y, unsafe)
+	unsafe = unsafe or false
 	local dx = x2 - x1
 	local dz = z2 - z1
-	local move_safe = dx ~= 0 or dz ~= 0
+	local move_safe = (not unsafe) and (dx ~= 0 or dz ~= 0)
+
 	if move_safe then
+		moveY(y1, safe_y)
+	else
+		moveY(y1, y2)
+	end
 
+	local newO = moveX(x1, x2, o1)
+	newO = moveZ(z1, z2, newO)
+
+	if move_safe then
+		moveY(safe_y, y2)
 	end
+
+	turnTo(newO, o2)
 end
 
-return { turnTo = turnTo, moveX = moveX }
+return { turnTo = turnTo, moveX = moveX, moveZ = moveZ, moveY = moveY, travel = travel }