local modem = peripheral.find("modem") or error("TURTLE HAS NO MODEM")
if (gps.locate(2, false) == nil) then
    error("GPS.LOCATE() IS NOT FUNCTIONING PROPERLY")
end

local HEADING = nil
local TURTLE_X, TURTLE_Y, TURTLE_Z = nil, nil, nil

-- North, East, South, West correlates to (1, 2, 3, 4)
local ACCEPTABLE_HEADINGS = {
    [1] = "-z",
    [2] = "+x",
    [3] = "+z",
    [4] = "-x"
}

function saveToFile()
    local TURTLE_FILE = fs.open("TURTLE_STATS", "w")
    local info = {TURTLE_X, TURTLE_Y, TURTLE_Z, HEADING}
    for key, line in ipairs(info) do
        TURTLE_FILE.writeLine(info[key])
    end
    TURTLE_FILE.close()
end

function clearPath()
    while (turtle.detect()) do
        turtle.dig()
    end
end

function locateCoordinates()
    TURTLE_X, TURTLE_Y, TURTLE_Z = gps.locate(2, false)
end

function getCoordinates()
    return TURTLE_X, TURTLE_Y, TURTLE_Z
end

function getCoordinatesFromFile()
    local TURTLE_FILE = fs.open("TURTLE_STATS", "r")
    TURTLE_X = TURTLE_FILE.readLine()
    TURTLE_Y = TURTLE_FILE.readLine()
    TURTLE_X = TURTLE_FILE.readLine()
    TURTLE_FILE.close()
    return TURTLE_X, TURTLE_Y, TURTLE_Z
end

function updateStats()
    locateCoordinates()
    local vectorOne = vector.new(getCoordinates())
    clearPath()
    if (forward()) then
        locateCoordinates()
        local vectorTwo = vector.new(getCoordinates())
        local resultVector = vectorTwo:sub(vectorOne)
        HEADING = ((math.abs(resultVector.x) * 3 - resultVector.x) + (math.abs(resultVector.z) * 2 + resultVector.z))
        back()
    else
        error("TURTLE DID NOT PROPERLY MOVE FORWARD")
    end
end

function getHeadingFromFile()
    local TURTLE_FILE = fs.open("TURTLE_STATS", "r")
    for a = 1, 3 do
        TURTLE_FILE.readLine()
    end
    HEADING = TURTLE_FILE.readLine()
    return HEADING
end

function faceHeading(finalHeading)
    local previousIndex
    if (HEADING == 1) then
        previousIndex = #ACCEPTABLE_HEADINGS
    else
        previousIndex = HEADING - 1
    end
    if (finalHeading == previousIndex) then
        turnLeft()
        return
    end
    local difference = math.abs(HEADING - finalHeading)
    while (difference > 0) do
        turnRight()
        difference = difference - 1
    end
end

function turnLeft()
    turtle.turnLeft()
    if (HEADING == 1) then
        HEADING = 4
    else
        HEADING = HEADING - 1
    end
    saveToFile()
end

function turnRight()
    turtle.turnRight()
    if (HEADING == 4) then
        HEADING = 1
    else
        HEADING = HEADING + 1
    end
    saveToFile()
end

function forward()
    if (turtle.forward()) then
        if (HEADING == 1) then
            TURTLE_Z = TURTLE_Z - 1
        elseif (HEADING == 2) then
            TURTLE_X = TURTLE_X + 1
        elseif (HEADING == 3) then
            TURTLE_Z = TURTLE_Z + 1
        elseif (HEADING == 4) then
            TURTLE_X = TURTLE_X - 1
        end
        saveToFile()
        return true
    end
    return false
end

function back()
    if (turtle.back()) then
        if (HEADING == 1) then
            TURTLE_Z = TURTLE_Z + 1
        elseif (HEADING == 2) then
            TURTLE_X = TURTLE_X - 1
        elseif (HEADING == 3) then
            TURTLE_Z = TURTLE_Z - 1
        elseif (HEADING == 4) then
            TURTLE_X = TURTLE_X + 1
        end
        saveToFile()
        return true
    end
    return false
end

function up()
    if (turtle.up()) then
        TURTLE_Y = TURTLE_Y + 1
        saveToFile()
        return true
    end
    return false
end

function down()
    if (turtle.down()) then
        TURTLE_Y = TURTLE_Y - 1
        saveToFile()
        return true
    end
    return false
end

function translateY(amount)
    local command
    if (amount < 0) then
        command = down
    else
        command = up
    end
    amount = math.abs(amount)
    while (amount > 0) do
        command()
        amount = amount - 1
    end
end

function translateX(amount)
    faceHeading(2)
    local command
    if (amount < 0) then
        command = back
    else
        command = forward
    end
    amount = math.abs(amount)
    while (amount > 0) do
        command()
        amount = amount - 1
    end
end

function translateZ(amount)
    faceHeading(3)
    local command
    if (amount < 0) then
        command = back
    else
        command = forward
    end
    amount = math.abs(amount)
    while (amount > 0) do
        command()
        amount = amount - 1
    end
end

function goToY(endY)
    local amount = endY - TURTLE_Y
    translateY(amount)
end

function goToX(endX)
    local amount = endX - TURTLE_X
    translateX(amount)
end

function goToZ(endZ)
    local amount = endZ - TURTLE_Z
    translateZ(amount)
end

function moveTo(x, y, z)
    local startVector = vector.new(getCoordinates())
    local endVector = vector.new(x, y, z)
    local midVector = endVector:sub(startVector)
    -- Take care of y first, then x, then z
    translateY(midVector.y)
    translateX(midVector.x)
    translateZ(midVector.z)
end

return { HEADING = HEADING, TURTLE_X = TURTLE_X, TURTLE_Y = TURTLE_Y, TURTLE_Z = TURTLE_Z, ACCEPTABLE_HEADINGS = ACCEPTABLE_HEADINGS, saveToFile = saveToFile, clearPath = clearPath, locateCoordinates = locateCoordinates, getCoordinates = getCoordinates, getCoordinatesFromFile = getCoordinatesFromFile, updateStats = updateStats, getHeadingFromFile = getHeadingFromFile, faceHeading = faceHeading, turnLeft = turnLeft, turnRight = turnRight, forward = forward, back = back, up = up, down = down, translateY = translateY, translateX = translateX, translateZ = translateZ, goToY = goToY, goToX = goToX, goToZ = goToZ, moveTo = moveTo }