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 expect = require("cc.expect")
local exp, field, range = expect.expect, expect.field, expect.range

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 turtleStats = fs.open("TURTLE_STATS", "w")
    local info = { TURTLE_X, TURTLE_Y, TURTLE_Z, HEADING }
    for key, line in ipairs(info) do
        turtleStats.writeLine(info[key])
    end
    turtleStats.close()
end

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

function clearPathUp()
    while (turtle.detectUp()) do
        turtle.digUp()
    end
end

function clearPathDown()
    while (turtle.detectDown()) do
        turtle.digDown()
    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 turtleStats = fs.open("TURTLE_STATS", "r")
    TURTLE_X = turtleStats.readLine()
    TURTLE_Y = turtleStats.readLine()
    TURTLE_X = turtleStats.readLine()
    turtleStats.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))
        if (not back()) then
            error("TURTLE DID NOT PROPERLY MOVE BACKWARDS")
        end
    else
        error("TURTLE DID NOT PROPERLY MOVE FORWARDS")
    end
end

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

function faceHeading(finalHeading)
    exp(1, finalHeading, "number")
    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 oppositeTurn(firstTurn)
    exp(1, firstTurn, "function")
    if (firstTurn == turnLeft) then
        return turnRight
    else
        return turnLeft
    end
end

function forward()
    clearPath()
    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 (not turtle.back()) then
        turtle.turnLeft()
        turtle.turnLeft()
        clearPath()
        turtle.turnLeft()
        turtle.turnLeft()
    else
        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()
    clearPathUp()
    if (turtle.up()) then
        TURTLE_Y = TURTLE_Y + 1
        saveToFile()
        return true
    end
    return false
end

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

function translateY(amount)
    exp(1, amount, "number")
    range(math.abs(amount), 0, turtle.getFuelLevel())
    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

-- SINCE U CANT BREAK BEHIND< MAKE TURTLE TURN AROUND AND BREAK BLOCKS IT DETECTS< THEN TURN AROUND AGAIN AFTER IT IS DONE!!!!!!!
function translateX(amount)
    exp(1, amount, "number")
    range(math.abs(amount), 0, turtle.getFuelLevel())
    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)
    exp(1, amount, "number")
    range(math.abs(amount), 0, turtle.getFuelLevel())
    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)
    exp(1, endY, "number")
    local amount = endY - TURTLE_Y
    translateY(amount)
end

function goToX(endX)
    exp(1, endX, "number")
    local amount = endX - TURTLE_X
    translateX(amount)
end

function goToZ(endZ)
    exp(1, endZ, "number")
    local amount = endZ - TURTLE_Z
    translateZ(amount)
end

function moveTo(x, y, z)
    exp(1, x, "number")
    exp(1, y, "number")
    exp(1, z, "number")
    goToY(y)
    goToX(x)
    goToZ(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 }