--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"
local MOVEHERE = "movehere"
-- Constant messaging
local LOG = "LOG"
local REQUEST = "REQ"
-- Constant requests
local BLOCKED = "blocked"
local ITEM = "item"
-- avoid breaking pattern
local notPattern = "^allthemodium"


-- 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 attemptBreak(breakf, inspectf, notPattern)
	local exists, data = inspectf()
	if not exists then
		return true
	end
	if string.match(data.name, notPattern) then
		return false
	end
	return breakf()
end

local function locatePosition()
	local x, y, z = gps.locate()
	if x == nil then return false end
	position = vector.new(x, y, z)
	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) then
			if ( heading ~= nil ) then
				position = position + heading
			else
				locatePosition()
			end
		end
		return true
	end
	return  false
end

local function attemptMoveBackward()
	if attemptMove(turtle.back) then
		relativePos = relativePos - relHeading
		if (position ~= nil) then
			if (heading ~= nil) then
				position = position - heading
			else
				locatePosition()
			end
		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 attemptBreakUp(notPattern)
	return attemptBreak(turtle.digUp, turtle.inspectUp, notPattern);
end

local function attemptBreakFront(notPattern)
	return attemptBreak(turtle.dig, turtle.inspect, notPattern)
end

local function attemptBreakDown(notPattern)
	return attemptBreak(turtle.digDown, turtle.inspectDown, notPattern)
end

local function orient(v, forward, right)
	v = v:normalize()
	if vectorEqual(v, forward) then
		return true
	elseif vectorEqual(v, right) then
		return attemptMoveRight()
	elseif vectorEqual(v, -right) then
		return attemptMoveLeft()
	elseif vectorEqual(v, -forward) then
		return attemptMoveRight() and attemptMoveRight()
	else
		sendLog(FAILED, "Improper axis given.");
	end
	return false
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 determinePosition()
	if not locatePosition() then
		sendLog(FAILED, "Unable to obtain GPS based position.")
		return
	end
	sendLog(POSITION, "Position obtained: " .. tostring(position))
end

local function determineHeading()
	sendLog(HEADING, "Calculating heading.")
	if position == nil and locatePosition() then
		sendLog(FAILED, "Unable to get 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
	local fDelta = position - oldPos
	
	if not attemptMoveBackward() then
		sendLog(FAILED, "Could not return to original position after first axis.")
		return
	end

	if not attemptMoveRight() then
		sendLog(FAILED, "Unable to rotate right.")
		return
	end
	
	if not attemptMoveForward() then
		sendLog(FAILED, "Unable to move forward after rotation.")
		return
	end
	local rDelta = position - oldPos
	if not attemptMoveBackward() then
		sendLog(FAILED, "Could not return to original position after second  axis.")
		return
	end
	if not attemptMoveLeft() then
		sendLog(FAILED, "Unable to rotate left.")
		return
	end

	heading = fDelta
	rightAxis = rDelta
	sendLog(HEADING, "Heading obtained. \nForward: " .. textutils.serialiseJSON(heading) .. "\nRight direction: " .. textutils.serialiseJSON(rightAxis))
end

local function moveBy(delta, relative)
    sendLog(MOVEBY, "Attempting to move by (" .. delta.x .. "," .. delta.y .. "," .. delta.z .. ") (x,y,z).")
	local forward
	local right
	if relative then
		forward = relHeading
		right = relRightAxis
	else
		forward = heading
		right = rightAxis
	end
	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
			if not attemptMoveForward() then
				sendLog(FAILED, "Unable to move forward.")
				return
			end
		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
			if not attemptMoveForward() then
				sendLog(FAILED, "Unable to move forward.")
				return
			end
		end
	end
	if delta.y > 0 then
		sendLog(MOVEBY, "Moving on y axis by: " .. delta.y)
		for y=1,delta.y do
			if not attemptMoveUp() then
				sendLog(FAILED, "Unable to move up.")
				return
			end
		end
	elseif delta.y < 0 then
		sendLog(MOVEBY, "Moving on y axis by: " .. delta.y)
		for y=1,math.abs(delta.y) do
			if not attemptMoveDown() then
				sendLog(FAILED, "Unable to move down.")
				return
			end
		end
	end
end

local function moveTo(dest, relative)
	local forward, right
	if relative then
		forward = relHeading
		right = relRightAxis
		pos = relativePos
	else
		forward = heading
		right = rightAxis
		pos = position
	end

	dest = dest:round()
    sendLog(MOVETO, "Attempting to move by (" .. dest.x .. "," .. dest.y .. "," .. dest.z .. ") (x,y,z).")
	if pos == nil or forward == nil then
		sendLog(FAILED, "Position or heading is invalid.")
		return
	end
	local xBlocked, yBlocked, zBlocked = false, false, false
	local oldPos = nil
	while not vectorEqual(dest, position) do
		local pos
		if relative then
			pos = relativePos
		else
			pos = position
		end
		if oldPos ~= nil and vectorEqual(oldPos, pos) then
			sendLog(FAILED, "Cannot proceed due to obstacle. XYZ being blocked: " .. tostring(xBlocked) .. "," .. tostring(yBlocked) .. "," .. tostring(zBlocked))
			break
		end
		oldPos = vector.new(pos.x, pos.y, pos.z);
		if position.x ~= dest.x then
			if not orient(vector.new(dest.x - pos.x, 0, 0):normalize(), forward, right) then
				sendLog(FAILED, "Unable to orient for X axis.")
				return
			end
			if relative then
				forward = relHeading
				right = relRightAxis
			else
				forward = heading
				right = rightAxis
			end
			xblocked = not attemptMoveForward()
		end

		if position.y ~= dest.y then
			if dest.y - pos.y > 0 then
				yBlocked = not attemptMoveUp()
			else
				yBlocked = not attemptMoveDown()
			end
		end
		
		if pos.z ~= dest.z then
			if not orient(vector.new(0, 0, dest.z - pos.z):normalize(), forward, right) then
				sendLog(FAILED, "Unable to orient for Z axis.")
				return
			end
			zBlocked = not attemptMoveForward()
		end
		if relative then
			forward = relHeading
			right = relRightAxis
		else
			forward = heading
			right = rightAxis
		end
	end
end

local function quarry(x, y, z, relative, pattern)
	x = tonumber(x)
	y = tonumber(y)
	z = tonumber(z)
	local forward;
	local right;
	if relative then
		forward = relHeading
		right = relRightAxis
	else
		forward = heading
		right = rightAxis
	end
	
	sendLog(QUARRY, "Starting quarry of (x length,depth,z length): " .. x .. "," .. y .. "," .. z .. " with a not pattern of: " .. pattern)
	for yDelta=1,math.abs(y) do
		if (y > 0) then
			if not attemptBreakUp(pattern) then
				sendLog(FAILED, "Unable to break up.")
				return
			end
			if not attemptMoveUp() then
				sendLog(FAILED, "Unable to move up.")
				return
			end
		else
			if not attemptBreakDown(pattern) then
				sendLog(FAILED, "Unable to break down.")
				return
			end
			if not attemptMoveDown() then
				sendLog(FAILED, "Unable to move further down for quarrying.")
				return
			end
		end
		local bool reverseTrip = false;
		for xDelta=1,math.abs(x) do
			for zDelta=1,(math.abs(z) - 1) do
				print("X,Z: " .. xDelta .. "," .. zDelta)
				print("heading, right heading: " .. textutils.serialiseJSON(forward) .. ", " .. textutils.serializeJSON(right))
				local success = true;
				if reverseTrip then
					orient(vector.new(0,0, -z), forward, right)
				else
					orient(vector.new(0, 0, z), forward, right)
				end
				if relative then
					forward = relHeading
					right = relRightAxis
				else
					forward = heading
					right = rightAxis
				end
				success = success and attemptBreakUp(pattern)
				success = success and attemptBreakDown(pattern)
				success = success and attemptBreakFront(pattern)
				success = success and attemptMoveForward(pattern)
				if not success then
					sendLog(FAILED, "Unable to proceed with quarrying.")
				end
			end
			reverseTrip = not reverseTrip
			if not (orient(vector.new(x, 0, 0), forward, right) and 
					attemptBreakFront(pattern) and 
					attemptBreakUp(pattern) and
					attemptBreakDown(pattern) and
					attemptMoveForward()) then
				sendLog(FAILED, "Unable to move to next row.")
				return
			end
			if relative then
				forward = relHeading
				right = relRightAxis
			else
				forward = heading
				right = rightAxis
			end
		end
	end
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 = nil
		if not pcall(function() data = textutils.unserialise(serialized) end) then
			print("Message already table, not deserializing.")
			data = serialized
		end
		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), false)
				else
					print("Using relative coordinates.")
					sendLog(MOVETO, "Using relative coordinates to move.")
					attemptStartTask(MOVETO, moveTo, vector.new(x,y,z), true)
				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), false)
				else
					print("Using relative coordinates.")
					sendLog(MOVEBY, "Using relative coordinates to move.")
					attemptStartTask(MOVEBY, moveBy, vector.new(x,y,z), true)
				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 and task["name"] or "nothing")
				}
				sendLog(STATUS, "Relative position: " .. textutils.serializeJSON(status["relative_position"]) .. 
								"\nPosition: " .. (status["position"]["coordinates"] and textutils.serialiseJSON(status["position"]) or "not positioned.") .. 
								"\nfuel: " .. tostring(status["fuel"]) .. 
								"\ncurrent task: " .. status["task"], status)
			elseif data["request"] == MOVEHERE then
				local x = data["content"][1]
				local y = data["content"][2]
				local z = data["content"][3]
				moveTo(vector.new(x, y, z), false)
			elseif data["request"] == POSITION then
				determinePosition()
			elseif data["request"] == HEADING then
				determineHeading()
			elseif data["request"] == QUARRY then
				if heading ~= nil then
					quarry(data["content"][1], data["content"][2], data["content"][3], false, notPattern)
					sendLog(QUARRY, "Using actual position for quarrying.")
				else
					sendLog(QUARRY, "Using relative position for quarrying.")
					quarry(data["content"][1], data["content"][2], data["content"][3], true, notPattern)
				end
			end
		end
	end
end

parallel.waitForAll(listen, executeTasks)