--configurables
local CHANNEL = 55555
local TOKEN = "LxdIjZPDLL2oZoayb7uR2Lbz7bnnbkxz"

-- Constant
local HOST = "HOST"
local UNIT = "LEAD"
local LUA_PREFIX = "?"
-- Constant actions
local MOVEBY = "moveby"
local MOVETO = "moveto"
local STATUS = "status"
local POSITION = "position"
local HEADING = "heading"
local QUARRY = "quarry"
-- Constant log
local LOG = "LOG"
local REQUEST = "REQ"
-- Constant requests
local BLOCKED = "blocked"
local ITEM = "item"

-- INIT SEQUENCE
print("Initiating leader mining turtle.")

running = true
local task = nil
local taskArgs = {}
local modem = nil
local heading = nil
local position = nil
local rightAxis = nil
local relHeading = vector.new(1,0,0)
local relativePos = vector.new(0,0,0)
local relRightAxis = vector.new(0,0,1)

do
    periphs = peripheral.find("modem")
    if not periphs then
        error("Modem peripheral not found.")
    else
        print("Modem found.")
        modem = periphs
    end
end
print("Opening channel: " .. CHANNEL)
modem.open(CHANNEL)

-- helpers
local function getMessage(data)
    if string.find(data, TOKEN .. ":") == 1 then
        print("Validated data token.")
        local validated = string.sub(data, string.len(TOKEN) + 1 + 1)
        print("Validated sequence: " .. validated)
        if string.find(validated, UNIT .. ":") == 1 then
        print("Checking unit...")
            return string.sub(validated, string.len(UNIT) + 1 + 1)
        end
    end
    print("Data was unable to be parsed.")
    return nil
end

local function SendRequest(request)
    print("Sending request: " .. request)
    modem.transmit(CHANNEL, CHANNEL, TOKEN .. ":" .. HOST .. ":" .. UNIT .. ":" .. REQUEST .. ":" .. request)
end

local function sendLog(log)
    print("Sending message: " .. log)
	modem.transmit(CHANNEL, CHANNEL, TOKEN .. ":" .. HOST .. ":" .. UNIT .. ":" .. LOG .. ":" .. log)
end

local function captureString(str, seq)
    if seq == nil then seq = "([^%s]+)" end
    res = {}
    for i in string.gmatch(str, seq) do
        table.insert(res, i)
    end
    return res
end

local function interpretData(data)
    res = captureString(data, "([^%\][^%:]+)")
    if res ~= nil then
        res[5] = table.concat(res, ":", 5)
        return res[1], res[2], res[3], res[4], res[5]
    end
    return nil
end

local function findItem(str)
	for s=1,16 do
		if string.sub(str, -string.len(str)) == str then
			return s
		end
	end
	return 0
end

local function refuel()
	local slot = findItem("coal")
	if slot == 0 then
		--TODO: request more coal
	end
	turtle.select(slot)
	turtle.refuel()
end

local function checkFuel()
	if turtle.getFuelLevel() > 0 then return false end
	refuel()
	return true;
end

local function vectorEqual(a, b)
    return a.x == b.x and a.y == b.y and a.z == b.z
end

local function attemptMove(movef)
	if not movef() then
		refuel()
		return movef();
	end
	return true
end

local function attemptMoveUp()
	if attemptMove(turtle.up) then
		relativePos.y = relativePos.y + 1
		if position ~= nil then position.y = position.y + 1 end
		return true
	end
	return  false
end

local function attemptMoveDown()
	if attemptMove(turtle.down) then
		relativePos.y = relativePos.y - 1
		if position ~= nil then position.y = position.y - 1 end
		return true
	end
	return  false
end

local function attemptMoveForward()
	if attemptMove(turtle.forward) then
		relativePos = relativePos + relHeading
		if (position ~= nil and heading ~= nil) then
			position = position + heading
		end
		return true
	end
	return  false
end

local function attemptMoveBackward()
	if attemptMove(turtle.back) then
		relativePos = relativePos - relHeading
		if (position ~= nil and heading ~= nil) then
			position = position - heading
		end
		return true
	end
	return  false
end

local function attemptMoveLeft()
	if attemptMove(turtle.turnLeft) then
		local previousHeading = relHeading
		relHeading = -relRightAxis
		relRightAxis = previousHeading
		
		if heading ~= nil then
			local previousHeading = heading
			heading = -rightAxis
			rightAxis = previousHeading
		end
		return true
	end
	return false
end

local function attemptMoveRight()
	if attemptMove(turtle.turnRight) then
		local previousHeading = relHeading
		relHeading = relRightAxis
		relRightAxis = -previousHeading
		
		if heading ~= nil then
			local previousHeading = heading
			heading = rightAxis
			rightAxis = -previousHeading
		end
		return true
	end
	return true
end

local function orient(v, forward, right)
    print("Attempting to orient such that " .. tostring(v) .. " == " .. tostring(forward) .. ".")
	if vectorEqual(v, forward) then
		return
	elseif vectorEqual(v, right) then
		attemptMoveRight()
	elseif vectorEqual(v, -right) then
		attemptMoveLeft()
	else
		attemptMoveRight()
		attemptMoveRight()
	end
	print("Orientation complete.")
end

local function attemptStartTask(f, ...)
	sendLog("attempting to start a task.")
	if task ~= nil then 
		sendLog("failed: unable to start task due to another task running.")
		return
	end
	task = f
	taskArgs = arg
end

-- actions

local function locatePosition()
	sendLog("position: using GPS to determine position.")
	local x, y, z = gps.locate()
	if x == nil then sendLog("failed: could not locate.") end
	position = vector.new(x, y, z)
end

local function determineHeading()
	sendLog("heading: calculating heading.")
	if position == nil then
		sendLog("failed: no current position")
		return
	end
	local oldPos = vector.new(position.x, position.y, position.z)
	if not attemptMoveForward() then
		sendLog("failed: could not move to get delta.")
		return
	end
	if x == nil then 
		sendLog("failed: could not locate.")
		return
	end
	heading = position - oldPos
	
	
	if not attemptMoveRight() then
		sendLog("failed: unable to rotate right.")
		heading = nil
		return
	end
	local oldPos = vector.new(position.x, position.y, position.z)
	if not attemptMoveForward() then
		sendLog("failed: unable to move forward after rotation.")
		heading = nil
		return
	end
	if not attemptMoveLeft() then
		sendLog("failed: unable to rotate left.")
		heading = nil
		return
	end
	rightAxis = position - oldPos
end

local function moveBy(delta, forward, right)
    sendLog("Attempting to move by (" .. delta.x .. "," .. delta.y .. "," .. delta.z .. ") (x,y,z).")
	-- x first
	if delta.x ~= 0 then 
		sendLog("Moving on x axis by: " .. delta.x)
		orient(vector.new(delta.x, 0, 0):normalize(), forward, right) 
		for x=1,math.abs(delta.x) do
			attemptMoveForward()
		end
	end
	if delta.z ~= 0 then 
		sendLog("Moving on z axis by: " .. delta.z)
		orient(vector.new(0, 0, delta.z):normalize(), forward, right)
		for z=1,math.abs(delta.z) do
			attemptMoveForward()
		end
	end
	if delta.y > 0 then
		sendLog("Moving on y axis by: " .. delta.y)
		for y=1,delta.y do
			attemptMoveUp()
		end
	elseif delta.y < 0 then
		sendLog("Moving on y axis by: " .. delta.y)
		for y=1,math.abs(delta.y) do
			attemptMoveDown()
		end
	end
end

local function quarry(a, b)
	sendLog("quarry: Starting quarry task from positions " .. tostring(a) .. " to " .. tostring(b) ".")
end

local function executeTasks()
	while running do
		if task ~= nil then
			task(unpack(taskArgs))
			task = nil
		end
		sleep(5)
	end
end

local function listen()
	while running do
		print("Listening for directives...")
		local event, side, sendChannel, replyChannel, data, distance = os.pullEvent("modem_message")
		print("Recieved data: " .. data)
		local token, recipient, sender, type, content = interpretData(data)
		if token == TOKEN and recipient == UNIT then
			print("Got content: " .. content)
			if string.find(content, LUA_PREFIX) == 1 then
				local luaScript = string.sub(content, string.len(LUA_PREFIX) + 1)
				print("Executing: " .. luaScript)
				local f = loadstring(luaScript)
				if f ~= nil then
					local freturn = f()
					
					if freturn ~= nil then
						modem.transmit(CHANNEL, CHANNEL, TOKEN .. ":" .. UNIT .. ":" ..  "obj" .. ":")
						modem.transmit(CHANNEL, CHANNEL, freturn)
					end
				else
					printError("Unable to load lua string.")
				end
			elseif string.find(content, MOVETO .. ":") == 1 then
				local coordsRaw = string.sub(content, string.len(MOVETO .. ":") + 1)
				print("Raw coordinate data: " .. coordsRaw)
				local coords = captureString(coordsRaw, "([+-]?%d+)")
				local x = coords[1]
				local y = coords[2]
				local z = coords[3]
				if heading ~= nil then
					attemptStartTask(moveTo, vector.new(x,y,z), heading, rightAxis)
				else
					attemptStartTask(moveTo, vector.new(x,y,z), relHeading, relRightAxis)
				end
			elseif string.find(content, MOVEBY .. ":") == 1 then
				local valuesRaw = string.sub(content, string.len(MOVEBY .. ":") + 1)
				print("Raw coordinate data: " .. valuesRaw)
				local deltas = captureString(valuesRaw, "([+-]?%d+)")
				local x = deltas[1]
				local y = deltas[2]
				local z = deltas[3]
				if heading ~= nil then
					print("Using actual coordinates.")
					attemptStartTask(moveBy, vector.new(x,y,z), heading, rightAxis)
				else
					print("Using relative coordinates.")
					attemptStartTask(moveBy, vector.new(x,y,z), relHeading, relRightAxis)
				end
			elseif string.find(content, STATUS) == 1 then
				status = {
					relative_position = {
						coordinates=relativePos,
						forward=relHeading,
						right=relRightAxis
					},
					position={
						coordinates=position,
						forward=heading,
						right=rightAxis
					},
					fuel=turtle.getFuelLevel(),
				}
				sendLog("status:" .. textutils.serialise(status))
			elseif string.find(content, POSITION) == 1 then
				locatePosition()
			elseif string.find(content, HEADING) == 1 then
				determineHeading()
			elseif string.find(content, QUARRY) == 1 then
			end
		end
	end
end

parallel.waitForAll(listen, executeTasks)