--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"
-- Constant log
local LOG = "LOG"
local REQUEST = "REQ"
-- Constant requests
local BLOCKED = "blocked"
local ITEM = "item"

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

local running = true
local task = nil
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
end

local function attemptStartTask(f, ...) {
    if (coroutine.status(task) ~= "dead") then 
        sendLog("failed: unable to start task due to another task running.")
        return false 
    end
    task = coroutine.create(f)
    table.remove(args, "n")
    task.resume(args)
    return true
}
-- actions

local function locatePosition()
	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()
	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 (" .. x .. "," .. y .. "," .. z .. ") (x,y,z).")
	-- x first
	if delta.x ~= 0 then orient(vector.new(delta.x, 0, 0):normalize(), forward, right) end
	for x=1,math.abs(delta.x) do
		attemptMoveForward()
	end
	if delta.z ~= 0 then orient(vector.new(0, 0, delta.z):normalize(), forward, right) end
	for z=1,math.abs(delta.z) do
		attemptMoveForward()
	end
	if delta.y > 0 then
		for z=1,delta.y do
			attemptMoveUp()
		end
	elseif delta.y < 0 then
		for z=1,math.abs(delta.y) do
			attemptMoveDown()
		end
	end
end

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.")
                moveBy(vector.new(x,y,z), heading, rightAxis)
            else
                print("Using relative coordinates.")
                moveBy(vector.new(x,y,z), relHeading, relRightAxis)
            end
		elseif string.find(content, STATUS .. ":") == 1 then
            table = {
                "relative_position"={
                    "coordinates"=relativePos,
                    "forward"=relHeading,
                    "right"=relRightAxis
                },
                "position"={
                    "coordinates"=position,
                    "forward"=heading,
                    "right"=rightAxis
                },
                "fuel"=turtle.getFuelLevel(),
                "task"=taskName
            }
            sendLog()
        end
    end
 end