pp = require("cc.pretty").pretty_print

_G.facing = "south"
_G.pos = vector.new(0,0,0)

local up = vector.new(0,1,0)

local rightOf = {
	south = "west",
	west = "north",
	north = "east",
	east = "south"
}

local leftOf = {
	west = "south",
	north = "west",
	east = "north",
	south = "east"
}

local vecOf = {
	north = vector.new(0,0,-1),
	south = vector.new(0,0,1),
	east = vector.new(1,0,0),
	west = vector.new(-1,0,0),
}

function goUp()
	if turtle.up() then
		_G.pos.y = _G.pos.y + 1
	else
		printError("failed to go up")
		printError(pos)
	end
end

function goDown()
	if turtle.down() then
		_G.pos.y = _G.pos.y - 1
	else
		printError("failed to go down")
		printError(pos)
	end
end

function goLeft()
	turtle.turnLeft()
	_G.facing = leftOf[_G.facing]
end

function goRight()
	turtle.turnRight()
	_G.facing = rightOf[_G.facing]
end

function goForward()
	if turtle.forward() then
		_G.pos = _G.pos + vecOf[_G.facing]
	else
		printError("failed to go forward")
		printError(pos)
	end
end

function goBack()
	if turtle.back() then
		_G.pos = _G.pos - vecOf[_G.facing]
	else
		printError("failed to go backward")
		printError(pos)
	end
end

function stepTo(target)
	local delta = target - _G.pos
	-- print(delta)
	if delta.y > 0 then
		goUp()
	elseif delta.y < 0 then
		goDown()
	elseif delta:dot(vecOf[_G.facing]) > 0 then
		goForward()
	elseif delta:dot(vecOf[_G.facing]:cross(up)) > 0 then
        goRight()
    else
        goLeft()
	end
end

function goTo(target, face)
	while target ~= _G.pos do
		stepTo(target)
	end
	if face and face ~= _G.facing then
		if rightOf[_G.facing] == face then
			goRight()
		elseif leftOf[_G.facing] == face then
			goLeft()
		else
			goRight()
			goRight()
		end
	end
end

function goHome()
	goTo(vector.new(0,0,0), "south")
end