pp = require("cc.pretty")

if _G.pos == nil then
	_G.facing = "south"
	_G.pos = vector.new(0,0,0)
	file = fs.open("pos.txt", "r")
	data = splitString(file.readAll())
	_G.pos.x = tonumber(data[1])
	_G.pos.y = tonumber(data[2])
	_G.pos.z = tonumber(data[3])
	_G.facing = data[4]
end

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 savePos()
	fs.delete("pos.txt")
	file = fs.open("pos.txt", "w")
	file.write(_G.pos.x .. " " .. _G.pos.y .. " " .. _G.pos.z .. " " .. _G.facing)
end


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

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

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

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

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

function goBack()
	if turtle.back() then
		_G.pos = _G.pos - vecOf[_G.facing]
		savePos()
	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 go_to(target, face)
	while target ~= _G.pos do
		stepTo(target)
		-- print(_G.pos, _G.facing, target, face)
		print(_G.pos, _G.facing)
	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