wirelessTurtle

module(...)

local U = require "utils"
local T = require "turtle"

rednet.open("right")

function goto(X, Y, Z)
	dirFacing = getDirFacing()
	cX, cY, cZ = gps.locate(10)
	upFirst = true
	if Y < cY then
		upFirst = false
	end
	dirsTried = 0
	while dirFacing == "?" do
		if upFirst and turtle.detectUp() then
			upFirst = false
			dirsTried = dirsTried + 1
		elseif not upFirst and turtle.detectDown() then
			upFirst = true
			dirsTried = dirsTried + 1
		end
		if dirsTried == 2 then
			return false
		end
		if upFirst then
			T.sUp()
		else
			T.sDown()
		end
		dirFacing = getDirFacing()
	end

	stack = { "$" }

	backtrack = false
	cX, cY, cZ = gps.locate(10)
	while not U.equal({cX, cY, cZ}, {X, Y, Z}) do
		optimal = findOptimal({cX, cY, cZ}, {X, Y, Z}, (backtrack and table.remove(stack, #stack) or nil), oposite(stack[#stack]))
		if backtrack and optimal == false then
			return false
		end

		if optimal == oposite(stack[#stack]) then

	end
end

function findOptimal(s, d, prevDir, backtrackDir)
	dx = {}
	for i = 1, 3, 1 do
		dx[i] = d[i] - s[i]
	end
	order = {}
	if math.abs(dx[1]) >= math.abs(dx[2]) and math.abs(dx[1]) >= math.abs(dx[3]) then
		if math.abs(dx[3]) >= math.abs(dx[2]) then
			order[1] = ((dx[1] > 0) and "+x" or "-x")
			order[2] = ((dx[3] > 0) and "+z" or "-z")
			order[3] = ((dx[2] > 0) and "+y" or "-y")
			order[4] = oposite(order[3]) order[5] = oposite(order[2]) order[6] = oposite(order[1])
		else
			order[1] = ((dx[1] > 0) and "+x" or "-x")
			order[2] = ((dx[2] > 0) and "+y" or "-y")
			order[3] = ((dx[3] > 0) and "+z" or "-z")
			order[4] = oposite(order[3]) order[5] = oposite(order[2]) order[6] = oposite(order[1])
		end
	elseif math.abs(dx[3]) >= math.abs(dx[2]) and math.abs(dx[3]) > math.abs(dx[1]) then
		if math.abs(dx[1]) >= math.abs(dx[2]) then
			order[1] = ((dx[3] > 0) and "+z" or "-z")
			order[2] = ((dx[1] > 0) and "+x" or "-x")
			order[3] = ((dx[2] > 0) and "+y" or "-y")
			order[4] = oposite(order[3]) order[5] = oposite(order[2]) order[6] = oposite(order[1])
		else
			order[1] = ((dx[3] > 0) and "+z" or "-z")
			order[2] = ((dx[2] > 0) and "+y" or "-y")
			order[3] = ((dx[1] > 0) and "+x" or "-x")
			order[4] = oposite(order[3]) order[5] = oposite(order[2]) order[6] = oposite(order[1])
		end
	elseif math.abs(dx[2]) > math.abs(dx[1]) and math.abs(dx[2]) > math.abs(dx[3]) then
		if math.abs(dx[1]) >= math.abs(dx[3]) then
			order[1] = ((dx[2] > 0) and "+y" or "-y")
			order[2] = ((dx[1] > 0) and "+x" or "-x")
			order[3] = ((dx[3] > 0) and "+z" or "-z")
			order[4] = oposite(order[3]) order[5] = oposite(order[2]) order[6] = oposite(order[1])
		else
			order[1] = ((dx[2] > 0) and "+y" or "-y")
			order[2] = ((dx[3] > 0) and "+z" or "-z")
			order[3] = ((dx[1] > 0) and "+x" or "-x")
			order[4] = oposite(order[3]) order[5] = oposite(order[2]) order[6] = oposite(order[1])
		end
	end

	if prevDir == nil && backtrackDir ~= order[1] then
		return order[1]
	end
	prevOrder = 1
	for i = 1, 6, 1 do
		if oreder[i] == prevDir then
			if i == 6 then
				return backtrackDir
			else
				if backtrackDir ~= order[i + 1] then
					return order[i + 1]
				else
					return ((i + 2 <= 6) and order[i + 2] or backtrackDir)
				end
			end
		end
	end
	return false
end

function oposite(x)
	if ascii(x) == '-' then
		return '+'..ascii(x, 2)
	elseif ascii(x) == '+' then
		return '-'..ascii(x, 2)
	end
	return x
end

function go(dir, dirFacing)
	if dir == "+y" then
		return T.sUp()
	elseif dir == "-y" then
		return T.sDown()
	end

	local dirs = { ["+x"] = 0, ["+z"] = 1, ["-x"] = 2, ["-z"] = 3 }

	if dir ~= dirFacing then
		turnDir = dirs[dir] - dirs[dirFacing]
		if turnDir <= 2 then
			T.turn("right", turnDir)
		else
			T.turn("left", 1)
		end
	end
	return T.sForward()
end

function getDirFacing()
	cX, cY, cZ = gps.locate(10)
	turnCount = 0
	while not T.sForward() do
		T.turn("right")
		turnCount = turnCount + 1
		if turnCount >= 4 then
			return "?"
		end
	end
	nX, nY, nZ = gps.locate(10)
	if nX ~= cX then
		if nX > cX then
			return "+x"
		else
			return "-x"
		end
	elseif nZ ~= cZ then
		if nZ > cZ then
			return "+z"
		else
			return "-z"
		end
	end
	return "?"
end