--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"
local TASK = "task"
local FAILED = "failed"
-- Constant messaging
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)
	reqData = {
		token=TOKEN,
		recipient=HOST,
		sender=UNIT,
		type=REQUEST,
		request=request,
		content=arg
	}
	modem.transmit(CHANNEL, CHANNEL, textutils.serialise(reqData))
end

local function sendLog(log, ...)
    print("Sending message: " .. log)
	logData = {
		token=TOKEN,
		recipient=HOST,
		sender=UNIT,
		type=LOG,
		log=log,
		content=arg
	}
	modem.transmit(CHANNEL, CHANNEL, textutils.serialise(logData))
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 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(name, f, ...)
	sendLog(TASK, "Attempting to start task.")
	if task ~= nil then 
		sendLog(FAILED, "Unable to start task due to another task running.")
		return
	end
	task = {name=name, func=f, args=arg}
	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(POSITION, "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(MOVEBY, "Attempting to move by (" .. delta.x .. "," .. delta.y .. "," .. delta.z .. ") (x,y,z).")
	-- x first
	if delta.x ~= 0 then 
		sendLog(MOVEBY, "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(MOVEBY, "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(MOVEBY, "Moving on y axis by: " .. delta.y)
		for y=1,delta.y do
			attemptMoveUp()
		end
	elseif delta.y < 0 then
		sendLog(MOVEBY, "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
			sendLog(TASK, "Executing task: " .. task["name"])
			task["func"](unpack(task["args"]))
			task = nil
		end
		sleep(5)
	end
end

local function listen()
	while running do
		print("Listening for directives...")
		local event, side, sendChannel, replyChannel, serialized, distance = os.pullEvent("modem_message")
		print("Recieved data...")
		local data = textutils.unserialise(serialized)
		if data["token"] == TOKEN and data["recipient"] == UNIT and data["type"] == REQUEST then
			print("Got request: " .. data["request"])
			
			if data["request"] == LUA_PREFIX then
				print("Executing: " .. data["content"][0])
				local f = loadstring(luaScript)
				if f ~= nil then
					local freturn = f()
					
					if freturn ~= nil then
						sendLog(LUA_PREFIX, textutils.serialise(freturn))
					end
				else
					printError("Unable to load lua string.")
				end
			elseif data["request"] == MOVETO then
				local x = data["content"][1]
				local y = data["content"][2]
				local z = data["content"][3]
				if heading ~= nil then
					print("Using actual coordinates.")
					sendLog(MOVETO, "Using actual coordinates to move.")
					attemptStartTask(MOVETO, moveTo, vector.new(x,y,z), heading, rightAxis)
				else
					print("Using relative coordinates.")
					sendLog(MOVETO, "Using relative coordinates to move.")
					attemptStartTask(MOVETO, moveTo, vector.new(x,y,z), relHeading, relRightAxis)
				end
			elseif data["request"] == MOVEBY then
				local x = data["content"][1]
				local y = data["content"][2]
				local z = data["content"][3]
				if heading ~= nil then
					print("Using actual coordinates.")
					sendLog(MOVEBY, "Using actual coordinates to move.")
					attemptStartTask(MOVEBY, moveBy, vector.new(x,y,z), heading, rightAxis)
				else
					print("Using relative coordinates.")
					sendLog(MOVEBY, "Using relative coordinates to move.")
					attemptStartTask(MOVEBY, moveBy, vector.new(x,y,z), relHeading, relRightAxis)
				end
			elseif data["request"] == STATUS then
				status = {
					relative_position = {
						coordinates=relativePos,
						forward=relHeading,
						right=relRightAxis
					},
					position={
						coordinates=position,
						forward=heading,
						right=rightAxis
					},
					fuel=turtle.getFuelLevel(),
					task=task["name"]
				}
				sendLog(STATUS, status)
			elseif data["request"] == POSITION then
				locatePosition()
			elseif data["request"] == HEADING then
				determineHeading()
			elseif data["request"] == QUARRY then
			end
		end
	end
end

parallel.waitForAll(listen, executeTasks)