local numutil = require 'numutil'

local function ensure_equipped(ptype, item)
	local cur = peripheral.getType"left"
	print(cur)
	if cur == ptype then return true end
	for i = 1,16 do
		local det = turtle.getItemDetail(i)
		if det and det.name == item then
			turtle.select(i)
			return turtle.equipLeft()
		end
	end
	return false, "couldn't find "..item
end

local function ensure_modem() return ensure_equipped("modem","computercraft:wireless_modem_normal") end
local function ensure_scanner() return error("aaa") end

local function turte_pos_init()
	assert(ensure_modem())

	local POS = vector.new(gps.locate())

	while turtle.detect() do turtle.turnRight() end
	turtle.forward()
	local newpos = vector.new(gps.locate())
	turtle.back()
	local FACING = newpos - POS

	return POS, FACING
end

local POS, FACING = turte_pos_init()

local function moveby_y(delta)
	local fn = turtle.up
	if delta < 0 then fn = turtle.down  delta = -delta  end
	for i=1,delta do fn() end
	POS.y = POS.y + delta
end

local function rotatev(v,nq)
	-- rotates v by n quarters, clockwise (rightwards)
	nq = nq%4
	if nq == 0     then return vector.new( v.x,  v.y,  v.z)
	elseif nq == 1 then return vector.new(-v.z,  v.y,  v.x)
	elseif nq == 2 then return vector.new(-v.x,  v.y, -v.z)
	elseif nq == 3 then return vector.new( v.z,  v.y, -v.x)
	end
end

local function turn_right()
	FACING = rotatev(FACING,1)
	turtle.turnRight()
end
local function turn_left()
	FACING = rotatev(FACING,-1)
	turtle.turnLeft()
end

local function turn_to_face(newfacing)
	if newfacing == FACING then return
	elseif newfacing == -FACING then turn_right() turn_right()
	elseif newfacing == rotatev(FACING,1) then turn_right()
	elseif newfacing == rotatev(FACING,-1) then turn_left()
	else error("invalid facing "..tostring(newfacing),2)
	end
end

local function move_linear(n)
	local dir = turtle.forward
	if n < 0 then dir = turtle.back end
	for i = 1,math.abs(n) do dir() end
	print(POS,n,FACING)
	POS = POS + FACING*n
end

local function moveby_xz(dv)
	if FACING.x ~= 0 then
		move_linear(dv:dot(FACING))
		if dv.z ~= 0 then
			turn_to_face(vector.new(0,0,numutil.sign(dv.z)))
			move_linear(dv:dot(FACING))
		end
	elseif FACING.z ~= 0 then
		move_linear(dv:dot(FACING))
		if dv.x ~= 0 then
			turn_to_face(vector.new(numutil.sign(dv.x),0,0))
			move_linear(dv:dot(FACING))
		end
	end
end

local function moveby(delta)
	moveby_y(delta.y)
	moveby_xz(delta)
end


local datum = vector.new(-18, 128, -1837)
local delta = datum - POS
moveby(delta)

for i=1,4 do
	move_linear(11)
	turn_right()
end