BranchMining - oc

-- Round to integers if within bounds
function round(num, bounds)
    if(num > 0) then
        if(num + bounds >= math.ceil(num)) then
            return math.ceil(num)
        elseif(num - bounds <= math.floor(num)) then
            return math.floor(num)
        end
    elseif(num < 0) then
        if(num - bounds <= math.floor(num)) then
            return math.floor(num)
        elseif(num + bounds >= math.ceil(num)) then
            return math.ceil(num)
        end
    end
    return num
end

-- Test if 2 points are equal in 3D
function testPointEquality(p1,p2)
    if(p1 == nil or p2 == nil) then
        return false
    end
    if(p1[2] == p2[2] and p1[3] == p2[3] and p1[4] == p2[4]) then
    return true
    else return false
    end
end

-- Obtain a table of 4 valid gps reference points
function getGPSCoordinates(timeOut)
    local timeOut = timeOut or 2
    local computer = require("computer")
    local event = require("event")
    local timer = computer.uptime()
    local points = {}
    while(computer.uptime() - timer < timeOut*4 and #points<4) do
        --print("Timer: "..computer.uptime() - timer)

        --[[    Data is assumed to be meaningful!
            This could be a huge problem, but will usually work if this
            port is only used for gps, a fix should be inplemented
        ]]--

        local eventType, recieverAddress, senderAddress, port, distance, x,y,z = event.pull(timeOut, "modem_message")
        if(distance ~= nil and x ~= nil and y ~= nil and z ~= nil) then
            local tmp = {distance, x,y,z}
            tmp = tonumberVector(tmp)
            local same = false
            for i=1, #points do
                if(testPointEquality(points[i], tmp)) then
                    same = true
                    --print("Same point!")
                    break
                end
            end
            if(not same) then
                if(#points<3) then
                    points[#points+1] = tmp
                    --print("Point added:"..tostring(tmp[1]))
                    --print(#points)
                else
                    local normPlane = getPlane(points[1],points[2],points[3])
                    --print(tostringVector(normPlane))
                    if(isInPlane(tmp,points[1],normPlane)) then
                        --print("Point in plane!")
                    else
                        --print("Point not in plane, adding point")
                        points[#points+1] = tmp
                        --print(#points)
                    end
                end
            end
        else
            print("Nil event type")
        end
    end
    --print(#points)
    if(#points >3) then
        return points
    else
        return nil
    end
end

-- Split a string into a table at occurrence of sep
function split(inputstr, sep)
    if(inputstr == nil or inputstr == "") then
                return nil
        end
        if sep == nil then
                sep = ","
        end
        local t={} ; i=1
        for str in string.gmatch(inputstr, "([^"..sep.."]+)") do
                t[i] = str
                i = i + 1
        end
        return t
end

-- Add two vectors
function addVector(p1,p2)
    --p1 + p2
    return {p1[1]+p2[1], p1[2]+p2[2], p1[3]+p2[3]}
end

-- Subtract two vectors
function subtractVector(p1,p2)
    --p1 - p2
    return {p1[1]-p2[1], p1[2]-p2[2], p1[3]-p2[3]}
end

-- Scale a vector
function scaleVector(p1, scalar)
    --p1 * scalar
    return {p1[1]*scalar, p1[2]*scalar, p1[3]*scalar}
end

-- Dot two vectors
function dotVector(p1,p2)
    return p1[1]*p2[1] + p1[2]*p2[2] + p1[3]*p2[3]
end

-- Cross two vectors
function crossVector(p1,p2)
    return {p1[2]*p2[3] - p1[3]*p2[2], p1[3]*p2[1] - p1[1]*p2[3], p1[1]*p2[2] - p1[2]*p2[1]}
end

-- Returns the magnitude of a vector
function magnitudeVector(p1)
    return math.sqrt(math.pow(p1[1], 2) + math.pow(p1[2], 2) + math.pow(p1[3], 2))
end

-- Normalize a vector
function normalizeVector(p1)
    return p1/magnitudeVector(p1)
end

-- Get a plane from 3 vectors
function getPlane(p1,p2,p3)
    --find the equation of the plane
    local p12 = subtractVector(p2,p1)
    local p13 = subtractVector(p3,p1)
    local plane = crossVector(p12,p13)
    return plane
end

-- Check if a vector is in a plane
function isInPlane(p1,p2,p3)
    local bounds= 0.000001
    --[[
        where:
        p1 is the point in question,
        p2 is a point known to exist on plane p3,
        and p3 is the plane in question
    ]]--
    if(dotVector(subtractVector(p1,p2),p3) < bounds and dotVector(subtractVector(p1,p2),p3) > -bounds) then
        return true
    else
        return false
    end
end

-- Rotate a vector around the Z axis
function rotateVectorZAxis(p1,theta)
    return {p1[1]*math.cos(theta) - p1[2]*math.sin(theta), p1[1]*math.sin(theta) + p1[2]*math.cos(theta), p1[3]}
end

-- Rotate a vector around the X axis
function rotateVectorXAxis(p1,theta)
    return {p1[1], p1[2]*math.cos(theta) - p1[3]*math.sin(theta), p1[2]*math.sin(theta) + p1[3]*math.cos(theta)}
end

-- Rotate a vector around the Y axis
function rotateVectorYAxis(p1,theta)
    return {p1[1]*math.cos(theta) + p1[3]*math.sin(theta), p1[2], p1[3]*math.cos(theta) - p1[1]*math.sin(theta)}
end

-- Turn a vector into a string
function tostringVector(p1)
    return tostring(p1[1]) .. "," .. tostring(p1[2]) .. "," .. tostring(p1[3])
end

-- Turn a vector into a number
function tonumberVector(p1)
    local tmp = {}
    for i=1, #p1 do
        tmp[i] = tonumber(p1[i])
    end
    return tmp
end

-- Round each entry of a vector using the round() function above
function roundVector(p1)
    local bounds = 0.000001
    local tmp = {}
    --[[
    for i=1, #p1 do
        if(p1[i] < bounds and p1[i] > -bounds) then
            tmp[i] = 0
        else
            tmp[i] = p1[i]
        end
    end
    ]]--
    for i=1, #p1 do
        tmp[i] = round(p1[i],bounds)
    end
    return tmp
end

-- Obtain the distance between two vectors
function distanceBetweenPoints(p1,p2)
    return math.sqrt(math.pow(p1[1]-p2[1],2) + math.pow(p1[2]-p2[2],2) + math.pow(p1[3]-p2[3],2))
end

-- 2D trilateration
function findPossiblePoints2D(d1,p1,d2,p2)
    local distance = distanceBetweenPoints(p1,p2)
    local a = (math.pow(d1, 2) - math.pow(d2, 2) + math.pow(distance, 2)) / (2 * distance)
    --print(a)
    local intersect = addVector(p1,scaleVector(scaleVector(subtractVector(p2,p1), a), (1/distance)))
    --print(tostringVector(intersect))
    local h = math.sqrt(math.pow(d1, 2) - math.pow(a, 2))
    --print(h)
    local x_plus = intersect[1] + ((h*(p2[2]-p1[2]))/distance)
    local x_minus = intersect[1] - ((h*(p2[2]-p1[2]))/distance)
    local y_plus = intersect[2] + ((h*(p2[1]-p1[1]))/distance)
    local y_minus = intersect[2] - ((h*(p2[1]-p1[1]))/distance)
    return {x_plus,y_minus,0}, {x_minus,y_plus,0}
end

-- 3D trilateration
function findPossiblePoints3D(d1,p1,d2,p2,d3,p3)

    --print("Trilateration points: "..d1..","..tostringVector(p1)..";"..d2..","..tostringVector(p2)..";"..d3..","..tostringVector(p3))

    --offset vectors such that p1 is at 0,0,0
    local offset = scaleVector(p1, -1)
    --print("Offset: "..tostringVector(offset))
    local p1p = addVector(p1, offset)
    --print("p1p: "..tostringVector(p1p))
    local p2p = addVector(p2, offset)
    --print("p2p: "..tostringVector(p2p))
    local p3p = addVector(p3, offset)
    --print("p3p: "..tostringVector(p3p))

    --print("Offset vectors: "..tostringVector(p1p).."; "..tostringVector(p2p).."; "..tostringVector(p3p))


    --find the equation of the plane
    local plane = roundVector(getPlane(p1p,p2p,p3p))
    --print("Plane: "..tostringVector(plane))

    -- checked up to here --

    --rotate first about the y axis such that p2 is at z = 0
    local angle1
    if(p2p[1] ~= 0)then
        angle1 = math.atan(p2p[3]/p2p[1])
    elseif(p2p[3] > 0) then
        --  90 degrees
        angle1 = 1.57079633
    elseif(p2p[3] < 0) then
        --  -90 degrees
        angle1 = -1.57079633
    end
    --print("Angle1: "..angle1)
    local plane_ry = roundVector(rotateVectorYAxis(plane, angle1))
    --print("plane_ry: "..tostringVector(plane_ry))
    local p1p_ry = roundVector(rotateVectorYAxis(p1p, angle1))
    --print("p1p_ry: "..tostringVector(p1p_ry))
    local p2p_ry = roundVector(rotateVectorYAxis(p2p, angle1))
    --print("p2p_ry: "..tostringVector(p2p_ry))
    local p3p_ry = roundVector(rotateVectorYAxis(p3p, angle1))
    --print("p3p_ry: "..tostringVector(p3p_ry))

    --rotate second about the z axis such that p2 is at y = 0 and the line from p1 to p2 is at z=0 y=0, and is hence on the x axis
    local angle2
    if(p2p_ry[1] ~= 0)then
        angle2 = (-1) * math.atan(p2p_ry[2]/p2p_ry[1])
    elseif(p2p_ry[2] > 0) then
        --  -90 degrees
        angle2 = -1.57079633
    elseif(p2p_ry[2] < 0) then
        --  90 degrees
        angle2 = 1.57079633
    end
    --print("Angle2: "..angle2)
    local plane_ryz = roundVector(rotateVectorZAxis(plane_ry, angle2))
    --print("plane_ryz: "..tostringVector(plane_ryz))
    local p1p_ryz = roundVector(rotateVectorZAxis(p1p_ry, angle2))
    --print("p1p_ryz: "..tostringVector(p1p_ryz))
    local p2p_ryz = roundVector(rotateVectorZAxis(p2p_ry, angle2))
    --print("p2p_ryz: "..tostringVector(p2p_ryz))
    local p3p_ryz = roundVector(rotateVectorZAxis(p3p_ry, angle2))
    --print("p3p_ryz: "..tostringVector(p3p_ryz))

    --rotate plane about the x axis such that p3 is at z = 0
    local angle3
    if(p3p_ryz[2] ~= 0)then
        angle3 = (-1) * math.atan(p3p_ryz[3]/p3p_ryz[2])
    elseif(p3p_ry[3] > 0) then
        --  -90 degrees
        angle3 = -1.57079633
    elseif(p3p_ry[3] < 0) then
        --  90 degrees
        angle3 = 1.57079633
    end
    --print("Angle3: "..angle3)
    local plane_ryzx = roundVector(rotateVectorXAxis(plane_ryz, angle3))
    --print("plane_ryzx: "..tostringVector(plane_ryzx))
    local p1p_ryzx = roundVector(rotateVectorXAxis(p1p_ryz, angle3))
    --print("p1p_ryzx: "..tostringVector(p1p_ryzx))
    local p2p_ryzx = roundVector(rotateVectorXAxis(p2p_ryz, angle3))
    --print("p2p_ryzx : "..tostringVector(p2p_ryzx ))
    local p3p_ryzx = roundVector(rotateVectorXAxis(p3p_ryz, angle3))
    --print("p3p_ryzx : "..tostringVector(p3p_ryzx ))

    --at this point, p1 should be at 0,0,0; p2 x,0,0; and p3 should be at x,y,0

    --calculations for finding rotated, offset points
    local xp_ryzx = (math.pow(d1,2) - math.pow(d2,2) + math.pow(p2p_ryzx[1],2))/(2*p2p_ryzx[1])
    local yp_ryzx = ((math.pow(d1,2) - math.pow(d3,2) + math.pow(p3p_ryzx[1],2) + math.pow(p3p_ryzx[2],2))/(2*p3p_ryzx[2])) - ((p3p_ryzx[1]/p3p_ryzx[2])*xp_ryzx)
    local z1p_ryzx = math.sqrt(math.pow(d1,2)-math.pow(xp_ryzx,2)-math.pow(yp_ryzx,2))
    local z2p_ryzx = (-1) * math.sqrt(math.pow(d1,2)-math.pow(xp_ryzx,2)-math.pow(yp_ryzx,2))

    --possible rotated, offset points
    local point1p_ryzx = {xp_ryzx, yp_ryzx, z1p_ryzx}
    local point2p_ryzx = {xp_ryzx, yp_ryzx, z2p_ryzx}

    --rotate back around the x axis
    plane_ryx = rotateVectorXAxis(plane_ryzx, (-1)*angle3)
    local point1p_ryz = rotateVectorXAxis(point1p_ryzx, (-1)*angle3)
    local point2p_ryz = rotateVectorXAxis(point2p_ryzx, (-1)*angle3)

    --rotate back around the z axis
    plane_ry = rotateVectorZAxis(plane_ryz, (-1)*angle2)
    local point1p_ry = rotateVectorZAxis(point1p_ryz, (-1)*angle2)
    local point2p_ry = rotateVectorZAxis(point2p_ryz, (-1)*angle2)

    --rotate back around the y axis
    plane = rotateVectorYAxis(plane_ry, (-1)*angle1)
    local point1p = rotateVectorYAxis(point1p_ry, (-1)*angle1)
    local point2p = rotateVectorYAxis(point2p_ry, (-1)*angle1)
    --print(tostringVector(plane))

    --remove offset
    local point1 = addVector(point1p, scaleVector(offset,-1))
    local point2 = addVector(point2p, scaleVector(offset,-1))

    --print("Possible points are either: "..tostringVector(point1).." or "..tostringVector(point2))
    return roundVector(point1), roundVector(point2)

end

-- Narrow trilateration points
function narrow(p1,p2,d4,p4)
    local bounds = 0.001
    local distance1 = distanceBetweenPoints(p1,p4)
    local error1 = distance1 - d4
    --print(error1)
    if(error1 < bounds and error1 > -bounds) then
        return p1
    end
    local distance2 = distanceBetweenPoints(p2,p4)
    local error2 = distance2 - d4
    --print(error2)
    if(error2 < bounds and error2 > -bounds) then
        return p2
    end
    print("Narrowing function could not obtain a meaningful answer")
    return nil
end

-- get gps coordinates
function getLocation(timeout)
    os.sleep(5)
    local timeout = timeout or 2
    local coords = getGPSCoordinates(timeout)
    if(coords == nil) then
        print("Coordinate table could not be obtained")
        return nil
    end
    if(#coords < 4) then
        print("Insufficient points")
        return nil
    end
    local d1 = coords[1][1]
    local p1 = {coords[1][2],coords[1][3],coords[1][4]}
    --print(tostringVector(p1))
    local d2 = coords[2][1]
    local p2 = {coords[2][2],coords[2][3],coords[2][4]}
    --print(tostringVector(p2))
    local d3 = coords[3][1]
    local p3 = {coords[3][2],coords[3][3],coords[3][4]}
    --print(tostringVector(p3))
    local d4 = coords[4][1]
    local p4 = {coords[4][2],coords[4][3],coords[4][4]}
    --print(tostringVector(p4))
    local point1, point2 = findPossiblePoints3D(d1,p1,d2,p2,d3,p3)
    --print("Possible point 1: "..tostringVector(point1))
    --print("Possible point 2: "..tostringVector(point2))
    local location = narrow(point1,point2,d4,p4)
    if(location ~= nil) then
        --print(tostringVector(location))
        return location
    else
        print("Location could not be narrowed")
        return nil
    end
end




-- Add elements to priorities list
function addList(name,mine,flag,drop,vein)
    local fileSystem = require("filesystem")
    local shell = require("shell")
    local xlist = {}
    local ifile
    if(fileSystem.exists(shell.resolve("listed"))) then
        --print("addList: File exists")
        ifile = io.open(shell.resolve("listed"), "r")
        i=1
        for line in io.lines(shell.resolve("listed")) do
            xlist[i] = split(line)
            i=i+1
        end

        for k,v in pairs(xlist) do
            if(name == v[1]) then
                --print("Ore already in list, not added")
                return false
            end
        end
        ifile:close()
        ifile = io.open(shell.resolve("listed"),"a")
    else
        ifile = io.open(shell.resolve("listed"),"w")
    end
    ifile:write(name..","..mine..","..flag..","..drop..","..vein.."\n")
    ifile:close()
    return true
end

-- Get priorities list in file format as a list
function lstToTable()
    local fileSystem = require("filesystem")
    local shell = require("shell")
    if(fileSystem.exists(shell.resolve("listed"))) then
        --print("lstToTable: File exists")
        ifile = io.open(shell.resolve("listed"),"r")
        xlist = {}
        for line in io.lines(shell.resolve("listed")) do
            table.insert(xlist, split(line))
        end
    else
        print("lstToTable: File doesn't exists")
        --      name,mine,flag,drop,vein
        xlist = {
            {'iron_ore','1','0','0','1'},
            {'coal_ore','1','0','0','1'},
            {'gold_ore','1','0','0','1'},
            {'lapis_ore','1','0','0','1'},
            {'redstone_ore','1','0','0','1'},
            {'lit_redstone_ore','1','0','0','1'},
            {'diamond_ore','1','0','0','1'},
            {'quartz_ore','1','0','0','1'},
            {'emerald_ore','1','0','0','1'}
            }
        for k,v in pairs(xlist) do
            addList(v[1],v[2],v[3],v[4],v[5])
        end
    end
    return xlist
end

-- Clear robot inventory according to priorities list
function clrInv(xtable)
    local robot = require("robot")
    local component = require("component")
    for i = 1, robot.inventorySize() do -- loop through the slots
        robot.select(i)
        local x = component.inventory_controller.getStackInInternalSlot(i)
        if(x ~= nil) then
            local istr = string.sub(x.name,string.find(x.name,":",0)+1)
            for key,value in pairs(xtable) do
                if(istr == value[1] and value[4] == "1") then
                    robot.drop()
                end
            end
        end
    end
    robot.select(1)
end

-- Sort robot inventory
function sortInv()
    local robot = require("robot")
    local component = require("component")
    for i = 1, robot.inventorySize() do -- loop through the slots
        robot.select(i)
        local x = component.inventory_controller.getStackInInternalSlot(i)
        if(x ~= nil) then
            for c = i, robot.inventorySize() do
                if(component.inventory_controller.getStackInInternalSlot(c) ~= nil) then
                    if(robot.compareTo(c)) then
                        robot.select(c)
                        robot.transferTo(i)
                        robot.select(i)
                    end
                end
            end
        end
    end
    robot.select(1)
end

-- Check for robot inventory space
function invSpace()
    local robot = require("robot")
    local component = require("component")
    i = robot.inventorySize()
    while(i>0) do
        robot.select(i)
        local x = component.inventory_controller.getStackInInternalSlot(i)
        if(x == nil or robot.compare()) then
            robot.select(1)
            return true
        end
        i = i-1
    end
    robot.select(1)
    return false
end

-- Check for sufficient fuel
function sufficientFuel() -- must figure out power consumption first, right not just set to half fuel left
    --params for later:
    --[[
        vector1,vector2,fuel,remainder
    ]]--
    local computer = require("computer")
    if(computer.energy()/computer.maxEnergy() < .5) then
        return false
    else
        return true
    end

    --[[
    local distance = math.abs(vector1.x - vector2.x) + math.abs(vector1.y - vector2.y) + math.abs(vector1.z - vector2.z)
    if(fuel <= distance + remainder) then
        return false
    else
        return true
    end
    ]]--
end

-- Consume robot fuel (requires generator upgrade) must be ported yet
function consumeFuel(maxFuel) -- Optionally add more fuel types and prioritise fuel types
    local refuel = false
    for i = 1, 16 do -- loop through the slots
        robot.select(i) -- change to the slot
        local x = robot.getItemDetail(i)
        if(x ~= nil) then
            local istr = string.sub(x.name,string.find(x.name,":",0)+1)
            if(istr == "planks" or istr == "stick" or istr == "log") then
                robot.refuel()
                refuel = true
            end
            if(robot.getFuelLevel() < maxFuel and istr == "coal") then
                robot.refuel(1)
                refuel = true
            end
        end
    end
    robot.select(1)
    --print(robot.getFuelLevel())
    return refuel
end

-- Place torch --must be ported yet
function placeTorch(direction) -- Optionally add functionality to place torch when no torch can be placed
    local robot = require("robot")
    local component = require("component")
    local torch = false
    for i = 1, robot.inventorySize() do -- loop through the slots
        robot.select(i) -- change to the slot
        local x = component.inventory_controller.getStackInInternalSlot(i)
        if(x ~= nil) then
            local istr = string.sub(x.name,string.find(x.name,":",0)+1)
            if(istr == "torch") then
                if(direction == "up") then
                    if(robot.placeUp()) then
                        torch = true
                        robot.select(1)
                    end
                elseif(direction == "down")then
                    if(robot.placeDown()) then
                        torch = true
                        robot.select(1)
                    end
                else
                    if(robot.place()) then
                        torch = true
                        robot.select(1)
                    end
                end
            end
        end
    end
    robot.select(1)
    return torch
end

-- Check weather or not the robot should continue essentially
function checkDependancies() -- Not implemented
    --must be implemented
        --things to include
        --[[
            - sufficient energy for action
            - tool durability
            -
        ]]--
end

-- Get robot heading
function getHeading() --needs refinement and entity detection
    local robot = require("robot")
    --[[
    --First check for required fuel
    if(robot.getFuelLevel() < 2) then
        if(not consumeFuel(400)) then
            error("Insufficient fuel")
        end
    end
    ]]--
    local start = getLocation()
    if(start == nil) then
        print("GPS coordinates could not be obtained")
        return nil
    else
        print(tostringVector(start))
    end
    while(robot.detect()) do
        robot.swing(3) --front
    end

    while (not robot.forward()) do end
    local current = getLocation()
    if(current == nil) then
        print("GPS coordinates could not be obtained")
        return nil
    else
        print(tostringVector(current))
    end
    robot.back()
    if(start[3] - current[3] > 0) then
        heading = 'N'
    elseif (start[3] - current[3] < 0) then
        heading = 'S'
    end
    if(start[1] - current[1] > 0) then
        heading = 'W'
    elseif (start[1] - current[1] < 0) then
        heading = 'E'
    end
    return heading
end

-- Set robot heading
function setHeading(heading,newHeading) -- needs optimization and more bug testing
    local robot = require("robot")
    if(heading ~= 'N' and heading ~= 'n' and heading ~= 'E' and heading ~= 'e' and heading ~= 'S' and heading ~= 's' and heading ~= 'W' and heading ~= 'w') then
        heading = getHeading()
    end
    if(heading ~= newHeading) then
        if(newHeading == 'N' or newHeading == 'n') then
            if(heading == 'S' or heading == 's') then
                robot.turnAround()
                return newHeading
            end
            if(heading == 'E' or heading == 'e') then
                robot.turnLeft()
                return newHeading
            end
            if(heading == 'W' or heading == 'w') then
                robot.turnRight()
                return newHeading
            end
        end
        if(newHeading == 'E' or newHeading == 'e') then
            if(heading == 'S' or heading == 's') then
                robot.turnLeft()
                return newHeading
            end
            if(heading == 'N' or heading == 'n') then
                robot.turnRight()
                return newHeading
            end
            if(heading == 'W' or heading == 'w') then
                robot.turnAround()
                return newHeading
            end
        end
        if(newHeading == 'S' or newHeading == 's') then
            if(heading == 'N' or heading == 'n') then
                robot.turnAround()
                return newHeading
            end
            if(heading == 'E' or heading == 'e') then
                robot.turnRight()
                return newHeading
            end
            if(heading == 'W' or heading == 'w') then
                robot.turnLeft()
                return newHeading
            end
        end
        if(newHeading == 'W' or newHeading == 'w') then
            if(heading == 'S' or heading == 's') then
                robot.turnRight()
                return newHeading
            end
            if(heading == 'E' or heading == 'e') then
                robot.turnAround()
                return newHeading
            end
            if(heading == 'N' or heading == 'n') then
                robot.turnLeft()
                return newHeading
            end
        end
    end
end

-- Go to a specific location
function goToLocation(location,heading) -- needs possibly entity detection, but probably not
    local robot = require("robot")
    if(heading ~= 'N' and heading ~= 'n' and heading ~= 'E' and heading ~= 'e' and heading ~= 'S' and heading ~= 's' and heading ~= 'W' and heading ~= 'w') then
        heading = getHeading()
        if(heading == nil) then
            print("No heading input and heading could not be obtained")
        else
            print("No heading input heading found to be: "..heading)
        end
    else
        print("Valid Heading")
    end
    print(tostringVector(location))
    local current = getLocation()
    print(tostringVector(current))
    local differance = subtractVector(location, current)
    print(tostringVector(differance))


    if(location[1] ~= current[1] or location[2] ~= current[2] or location[3] ~= current[3]) then
        if(not sufficientFuel()) then
            error("Insufficient fuel")
        end
    end
    -- y value
    while(differance[2] ~= 0) do
        if(differance[2] > 0) then
            while(robot.detectUp()) do
                robot.swingUp()
            end
            robot.up()
            differance[2] = differance[2] - 1
        else
            while(robot.detectDown()) do
                robot.swingDown()
            end
            robot.down()
            differance[2] = differance[2] + 1
        end
        print(tostringVector(differance))
    end
    -- z value
    while(differance[3] ~= 0) do
        if(differance[3] > 0) then
            setHeading(heading,'S')
            heading = 'S'
            while(robot.detect()) do
                robot.swing(3)
            end
            robot.forward()
            differance[3] = differance[3] - 1
        else
            setHeading(heading,'N')
            heading = 'N'
            while(robot.detect()) do
                robot.swing(3)
            end
            robot.forward()
            differance[3] = differance[3] + 1
        end
        print(tostringVector(differance))
    end
    -- x value
    while(differance[1] ~= 0) do
        if(differance[1] > 0) then
            setHeading(heading,'E')
            heading = 'E'
            while(robot.detect()) do
                robot.swing(3)
            end
            robot.forward()
            differance[1] = differance[1] - 1
        else
            setHeading(heading,'W')
            heading = 'W'
            while(robot.detect()) do
                robot.swing(3)
            end
            robot.forward()
            differance[1] = differance[1] + 1
        end
        print(tostringVector(differance))
    end
    return heading
end


-- Decide whether to mine whole ore vein based on priorities list
function shouldMineWhole(istr,xlist) -- Optionally add functionality for flagging ores
    for key,value in pairs(xlist) do
        if(istr == value[1] and value[5] == "1") then
            return true
        end
    end
    return false
end






function mTunnel(startingLocation, tunnelHeading, tunnelLength, currentLocation, currentHeading, pList, branchSpacing,torchSpacing)
    -- Function dependencies --
    local robot = require("robot")
    -- Preliminary Checks --

    -- Function Variables --
    local currentTmp
    if(startingLocation == nil or currentLocation == nil) then
        currentTmp = getLocation()
    end

    if(startingLocation == nil) then
        startingLocation = currentTmp
        if(startingLocation == nil) then
            error("No starting location input, and gps could not find signal to default to current location")
        else
            print("Starting Location defaulting to current location: "..tostringVector(startingLocation))
        end
    else
        print("Starting Location input at: "..tostringVector(startingLocation))
    end

    if(currentLocation == nil) then
        currentLocation = currentTmp
        if(currentLocation == nil) then
            error("No current location input, and gps could not find signal to default to current location")
        else
            print("Current Location defaulting to current location: "..tostringVector(currentLocation))
        end
    else
        print("Current Location input at: "..tostringVector(currentLocation))
    end

    local headingTmp
    if(tunnelHeading == nil or currentHeading == nil) then
        headingTmp = getHeading()
    end

    if(tunnelHeading == nil) then
        tunnelHeading = headingTmp
        if(tunnelHeading == nil) then
            error("No tunnel heading input, and gps could not find signal to default to current heading")
        else
            print("Tunnel Heading defaulting to current Heading: "..tunnelHeading)
        end
    else
        print("Tunnel Heading input at: "..tunnelHeading)
    end

    if(currentHeading == nil) then
        currentHeading = headingTmp
        if(currentHeading == nil) then
            error("No current heading input, and gps could not find signal to default to current heading")
        else
            print("Current Heading defaulting to current Heading: "..currentHeading)
        end
    else
        print("Current Heading input at: "..currentHeading)
    end

    if(tunnelLength == nil) then
        tunnelLength = 64
        print("No tunnel length input, defaulting to: "..tunnelLength)
    else
        print("Tunnel length input at: "..tunnelLength)
    end

    if(branchSpacing == nil) then
        branchSpacing = 4
        print("No Branch Spacing input, defaulting to: "..branchSpacing)
    else
        print("Branch Spacing input at: "..branchSpacing)
    end

    if(torchSpacing == nil) then
        torchSpacing = 4
        print("No Torch Spacing input, defaulting to: "..torchSpacing)
    else
        print("Torch Spacing input at: "..torchSpacing)
    end

    local done = 0
    local stop = false
    local torches = true
    local gps_range = true
    local distanceZ = math.abs(startingLocation[3] - currentLocation[3])
    local distanceX = math.abs(startingLocation[1] - currentLocation[1])
    while done == 0 do
        -- Along the North/South line --
        if(tunnelHeading == 'N' or tunnelHeading == 'n' or tunnelHeading == 'S' or tunnelHeading == 's')then
            print("Tunnel Heading is along the North/South line (z axis) with heading: "..tunnelHeading)
            -- Check if distance from tunnelStart is within tunnelLength --
            if(distanceZ >= tunnelLength) then
                -- Distance from tunnelStart is not within tunnelLength --
                -- Most likely the mining is done
                -- Go back
                currentHeading = goToLocation(startingLocation,currentHeading)
                done = 1 -- limit reached
            else
                -- Distance from tunnelStart is within tunnelLength --
                -- Run some checks for:
                --[[
                    insufficient fuel error code 2
                    insufficient inventory space error code 3
                    insufficient durability error code 4
                    stop signal received error code 5
                    insufficient torches error code 6
                    within adequate gps range error code 7
                    other checks?
                ]]--
                if(sufficientFuel()) then
                    -- There is sufficient fuel
                    if(invSpace()) then
                        -- There is sufficient inventory space

                        local durability, damage = robot.durability()
                        if(durability == nil) then
                            durability = 0
                        end
                        if(durability > 0.1 or damage == "tool cannot be damaged") then
                            -- There is sufficient durability
                            if(not stop) then
                                -- No stop signal received
                                if(torches) then
                                    -- There is sufficient torches
                                    if(gps_range) then
                                        -- Within adequate gps range
                                        print("All conditions met")
                                        print(distanceZ)

                                        -- mining algorithm for a 3x3 tunnel
                                        -- Tunnel is just starting
                                        if(distanceZ == 0) then
                                            --[[
                                                XXX
                                                XXX
                                                XOX
                                            ]]--
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            --[[
                                                XMX
                                                XOX
                                                XMX
                                            ]]--
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            robot.up()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            robot.turnRight()
                                            --[[
                                                XMM
                                                XMO
                                                XMM
                                            ]]--
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            robot.turnAround()
                                            robot.forward()
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            robot.turnRight()
                                            distanceZ = distanceZ + 1
                                        --tunnel is at an odd distance
                                        elseif(distanceZ%2 == 1) then
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            if(distanceZ%branchSpacing == 0) then
                                                robot.turnLeft()
                                                placeTorch("up")
                                                robot.swing(3)
                                                robot.turnRight()
                                            end
                                            robot.turnRight()
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            if(distanceZ%branchSpacing == 0) then
                                                placeTorch("up")
                                                robot.swing(3)
                                            end
                                            robot.turnLeft()
                                            distanceZ = distanceZ + 1
                                        --tunnel is at an even distance
                                        else
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            if(distanceZ%branchSpacing == 0) then
                                                robot.turnRight()
                                                placeTorch("up")
                                                robot.swing(3)
                                                robot.turnLeft()
                                            end
                                            robot.turnLeft()
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            while(robot.detect()) do
                                                robot.swing(3)
                                            end
                                            robot.forward()
                                            while(robot.detectUp()) do
                                                robot.swingUp()
                                            end
                                            while(robot.detectDown()) do
                                                robot.swingDown()
                                            end
                                            if(distanceZ%branchSpacing == 0) then
                                                placeTorch("up")
                                                robot.swing(3)
                                            end
                                            robot.turnRight()
                                            distanceZ = distanceZ + 1
                                        end
                                    else
                                        -- Not within adequate gps range
                                        -- Go Back
                                        currentHeading = goToLocation(startingLocation,currentHeading)
                                        done = 7
                                        break
                                    end

                                else
                                    -- There is not sufficient torches
                                    -- Try to do something about it

                                    if(not torches) then
                                        -- Nothing could be done
                                        -- Go Back
                                        currentHeading = goToLocation(startingLocation,currentHeading)
                                        done = 6
                                        break
                                    else

                                    end
                                end
                            else
                                -- Stop signal received
                                -- Interpret the data and see what needs doing
                                done = 5
                            end
                        else
                            -- There is not sufficient durability
                            -- Try to do something about it

                            if(not (durability > 0.1 or damage == "tool cannot be damaged")) then
                                -- Nothing could be done
                                -- Go back
                                currentHeading = goToLocation(startingLocation,currentHeading)
                                done = 4 -- not sufficient durability
                                break
                            else
                                -- Something could be done, great
                            end
                        end
                    else
                        -- There is not sufficient inventory space
                        -- Try to do something about it

                        sortInv()
                        clrInv(pList)
                        if(not invSpace()) then
                            -- Nothing could be done
                            -- Go back
                            currentHeading = goToLocation(startingLocation,currentHeading)
                            done = 3 -- not sufficient inventory space
                            break
                        else
                            -- something could be done, great
                        end
                    end
                else
                    -- There is not sufficient fuel
                    -- Try to do something about it

                    --[[
                        consume fuel function
                    ]]--

                    if(not sufficientFuel()) then
                        -- Nothing could be done
                        -- Go back
                        currentHeading = goToLocation(startingLocation,currentHeading)
                        done = 2 -- Not sufficient fuel
                        break
                    else
                        -- something could be done, great
                    end
                end
            end
        -- Along the East/West line --
        elseif(tunnelHeading == 'E' or tunnelHeading == 'e' or tunnelHeading == 'W' or tunnelHeading == 'w') then
            print("Tunnel Heading is along the East/West line (x axis) with heading: "..tunnelHeading)
            -- Check if distance from tunnelStart is within tunnelLength --
            if(distanceX >= tunnelLength) then
                -- Distance from tunnelStart is not within tunnelLength --
                -- Most likely the mining is done
                -- Go Back
                currentHeading = goToLocation(startingLocation,currentHeading)
                return 1 -- limit reached
            else
                -- Distance from tunnelStart is within tunnelLength --
                -- Run some checks for:
                --[[
                    sufficient fuel error code 2
                    sufficient inventory space error code 3
                    sufficient durability error code 4
                    stop signal received error code 5
                    sufficient torches error code 6
                    within adequate gps range error code 7
                    other checks?
                ]]--
                if(sufficientFuel()) then
                    -- There is sufficient fuel
                    if(invSpace()) then
                        -- There is sufficient inventory space

                        local durability, damage = robot.durability()
                        if(durability == nil) then
                            durability = 0
                        end
                        if(durability > 0.1 or damage == "tool cannot be damaged") then
                            -- There is sufficient durability
                            if(not stop) then
                                -- No stop signal received
                                if(torches) then
                                    -- There is sufficient torches
                                    if(gps_range) then
                                        -- Within adequate gps range
                                        print("All conditions met")
                                        --  mining algorithm for a 3x3 tunnel



                                    else
                                        -- Not within adequate gps range
                                        -- Go Back
                                        currentHeading = goToLocation(startingLocation,currentHeading)
                                        done = 7
                                        break
                                    end

                                else
                                    -- There is not sufficient torches
                                    -- Try to do something about it

                                    if(not torches) then
                                        -- Nothing could be done
                                        -- Go Back
                                        currentHeading = goToLocation(startingLocation,currentHeading)
                                        done = 6
                                        break
                                    else

                                    end
                                end
                            else
                                -- Stop signal received
                                -- Interpret the data and see what needs doing
                                done = 5
                            end
                        else
                            -- There is not sufficient durability
                            -- Try to do something about it

                            if(not (durability > 0.1 or damage == "tool cannot be damaged")) then
                                -- Nothing could be done
                                -- Go back
                                currentHeading = goToLocation(startingLocation,currentHeading)
                                done = 4 -- not sufficient durability
                                break
                            else
                                -- Something could be done, great
                            end
                        end
                    else
                        -- There is not sufficient inventory space
                        -- Try to do something about it

                        sortInv()
                        clrInv(pList)
                        if(not invSpace()) then
                            -- Nothing could be done
                            -- Go back
                            currentHeading = goToLocation(startingLocation,currentHeading)
                            done = 3 -- not sufficient inventory space
                            break
                        else
                            -- something could be done, great
                        end
                    end
                else
                    -- There is not sufficient fuel
                    -- Try to do something about it

                    --[[
                        consume fuel function
                    ]]--

                    if(not sufficientFuel()) then
                        -- Nothing could be done
                        -- Go back
                        currentHeading = goToLocation(startingLocation,currentHeading)
                        done = 2 -- Not sufficient fuel
                        break
                    else
                        -- something could be done, great
                    end
                end
            end
        end
    end -- End main loop

    return heading, done
end


local component = require("component")
local robot = require("robot")

local gps_port = 3665
component.modem.open(gps_port)
--[[
print("Sleep: ")
local s = tonumber(io.read())
for i=1, tonumber(io.read()) do
    print(tostringVector(getLocation()))
    robot.forward()
end
]]--

--[[
local coords = getLocation()
local headig = getHeading()
print("Starting at: "..tostringVector(coords))
local diff = {0,0,5}
heading = goToLocation(addVector(coords,diff),headig)
print("At location: "..tostringVector(getLocation()))
heading = goToLocation(coords,heading)
print("At location: "..tostringVector(getLocation()))
]]--


local start = getLocation()
local heading, done = mTunnel(start, nil, 8, nil, nil, lstToTable(), nil)
print("Heading: "..heading.." Done: "..done)