move to coordinates (gps based)

local args = { ... }

function loc()
	return vector.new(gps.locate(5))
end

-- function setOrientationToPlusX()
	-- while true do
		-- local old = loc()
		-- if not (turtle.forward()) then
			-- turtle.dig()
			-- turtle.forward()
		-- end
		-- local new = loc()
		-- turtle.back()
		-- turtle.place()
		-- local dif = old - new
		-- if dif.x == 1 then
			-- break
		-- else
			-- turtle.turnRight()
		-- end
	-- end
-- end

function forwardX()
	turtle.down()
	while not turtle.detectDown() do
		turtle.down()
	end
	while not turtle.forward() do
		while not turtle.up() do
			while not turtle.back() do
				correctY()
				return
			end
		end
	end
end

function forwardY()
	turtle.down()
	while not turtle.detectDown() do
		turtle.down()
	end
	while not turtle.forward() do
		while not turtle.up() do
			while not turtle.back() do
				return
			end
		end
	end
end

function moveBy (x, y) -- no specific z movement
	correctH(1)
	if x > 0 then
		for i = 1, x, 1 do
			forward()
		end
	end
	if x < 0 then
		turtle.turnRight()
		turtle.turnRight()
		for i = -1, x, -1 do
			forward()
		end
		turtle.turnLeft()
		turtle.turnLeft()
	end
	if y > 0 then
		turtle.turnRight()
		for j = 1, y, 1 do
			forward()
		end
		turtle.turnLeft()
	end
	if y < 0 then
		turtle.turnLeft()
		for j = -1, y, -1 do
			forward()
		end
		turtle.turnRight()
	end
end

function correctX()
	if loc().x > target.x then
		targetH = 1
	end
	if loc().x < target.x then
		targetH = 3
	end

	if loc().x == target.x then
		return
	end

	correctH(targetH)
	while true do
		if loc().x == target.x then
			return
		else
			forwardX()
		end
	end
end

function correctY()
	if loc().y > target.y then
		targetH = 2
	end
	if loc().y < target.y then
		targetH = 0
	end

	if loc().y == target.y then
		return
	end

	correctH(targetH)
	while true do
		if loc().y == target.y then
			return
		else
			forwardY()
		end
	end
end

function correctZ()
	while true do
		if loc().z > target.z then
			if not turtle.down() then
				--turtle.digDown()
				turtle.down()
			end
		end
		if loc().z < target.z then
			if not turtle.up() then
				--turtle.digUp()
				turtle.up()
			end
		end
		if loc().z == target.z then
			return
		end
	end
end

function correctH(h)
	old = loc()
	while turtle.detect() do
		turtle.turnRight()
	end
	turtle.forward()
	new = loc()
	turtle.back()
	diff = old - new
	-- (0, -1, 0) is f = 0
	-- (1, 0, 0) is f = 1
	-- (0, 1, 0) is f = 2
	-- (-1, 0, 0) is f = 3
	if diff.y == -1 then
		f = 0
	end
	if diff.x == 1 then
		f = 1
	end
	if diff.y == 1 then
		f = 2
	end
	if diff.x == -1 then
		f = 3
	end
	while true do
		if f == h then
			return
		end
		f = f + 1
		turtle.turnRight()
		if f > 3 then
			f = 0
		end
	end
end

function move()
	correctX()
	correctY()
	correctZ()
	correctH(h)
end

x = tonumber(args[1])
y = tonumber(args[2])
z = tonumber(args[3])
h = tonumber(args[4])
target = vector.new(x, y, z)
--moveBy (diff.x, diff.y)
move()
correctZ()
correctH(h)