posLib

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 saveStats()
    local turtleStats = fs.open("TURTLE_STATS", "w")
    local info = { TURTLE_X, TURTLE_Y, TURTLE_Z, HEADING }
    for _, value in ipairs(info) do
        turtleStats.writeLine(value)
    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 = tonumber(turtleStats.readLine())
    TURTLE_Y = tonumber(turtleStats.readLine())
    TURTLE_X = tonumber(turtleStats.readLine())
    turtleStats.close()
    return TURTLE_X, TURTLE_Y, TURTLE_Z
end

function getXFromFile()
    local turtleStats = fs.open("TURTLE_STATS", "r")
    TURTLE_X = tonumber(turtleStats.readLine())
    return TURTLE_X
end

function getYFromFile()
    local turtleStats = fs.open("TURTLE_STATS", "r")
    turtleStats.readLine()
    TURTLE_Y = tonumber(turtleStats.readLine())
    return TURTLE_Y
end

function getZFromFile()
    local turtleStats = fs.open("TURTLE_STATS", "r")
    turtleStats.readLine()
    turtleStats.readLine()
    TURTLE_Z = tonumber(turtleStats.readLine())
    return TURTLE_Z
end

function getHeading()
    return HEADING
end

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

function calculateStats()
    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
        saveStats()
    else
        error("TURTLE DID NOT PROPERLY MOVE FORWARDS")
    end
end

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

function turnLeft()
    turtle.turnLeft()
    if (HEADING == 1) then
        HEADING = #ACCEPTABLE_HEADINGS
    else
        HEADING = HEADING - 1
    end
    saveStats()
end

function turnRight()
    turtle.turnRight()
    if (HEADING == #ACCEPTABLE_HEADINGS) then
        HEADING = 1
    else
        HEADING = HEADING + 1
    end
    saveStats()
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
        saveStats()
        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
        saveStats()
        return true
    end
    return false
end

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

function down()
    clearPathDown()
    if (turtle.down()) then
        TURTLE_Y = TURTLE_Y - 1
        saveStats()
        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")
    if (TURTLE_Y == endY) then return end
    local amount = endY - getYFromFile()
    translateY(amount)
end

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

function goToZ(endZ)
    exp(1, endZ, "number")
    if (TURTLE_Z == endZ) then return end
    local amount = endZ - getZFromFile()
    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 { saveStats = saveStats, clearPath = clearPath,
    clearPathUp = clearPathUp,
    clearPathDown = clearPathDown, locateCoordinates = locateCoordinates, getCoordinates = getCoordinates,
    getCoordinatesFromFile = getCoordinatesFromFile, getXFromFile = getXFromFile, getYFromFile = getYFromFile,
    getZFromFile = getZFromFile, getHeading = getHeading, getHeadingFromFile = getHeadingFromFile,
    calculateStats = calculateStats, 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 }