an

local nav = nil
local navDir = 1
local verbose = true
local slotSel = 1
local baseData = {}
local roamingData = {selSlot=1}
turtle.select(1)

local gpsDataFile = "gpsData"
local slotFile = "slotFile"
local baseDataFile = "baseData"
local roamingDataFile = "roamingData"

local vecVars = {"x","y","z"}

local vecMods = {}
vecMods[1] = vector.new(0,0,1)
vecMods[2] = vector.new(-1,0,0)
vecMods[3] = vector.new(0,0,-1)
vecMods[4] = vector.new(1,0,0)
vecMods[5] = vector.new(0,1,0)
vecMods[6] = vector.new(0,-1,0)

function vectorFromTable(tab)
    return vector.new(tab.x,tab.y,tab.z)
end

function tableToFile(fileName,saveTable)
    local f = fs.open(fileName,"w")
    f.writeLine(textutils.serialize(saveTable))
    f.close()
end

function tableFromFile(fileName)
    local f = fs.open(fileName,"r")
    if not f then return nil end
    local t = textutils.unserialize(f.readLine())
    f.close()
    return t
end

function saveBaseData()
    tableToFile(baseDataFile,baseData)
end

function loadBaseData()
    return tableFromFile(baseDataFile)
end

function getRoamingData(vName)
    return roamingData[vName]
end

function getBaseData(vName)
    return baseData[vName]
end

function saveRoamingData()
    tableToFile(roamingDataFile,roamingData)
end
if not fs.exists(roamingDataFile) then saveRoamingData() end

function loadRoamingData()
    return tableFromFile(roamingDataFile)
end

function setBaseData(bName,bVal)
    print(tostring(bName).."/"..tostring(bVal))
    baseData[bName] = bVal
    saveBaseData()
end



function setRoamingData(bName,bVal,forceRAM)
    forceRAM = forceRAM or false
    roamingData[bName] = bVal

    if not forceRAM then

        saveRoamingData()
    end
end

function saveSlot()
    setRoamingData("selSlot",selSlot)
end

function loadSlot()
    return roamingData.selSlot
end

function select(nSlot)
    slotSel = nSlot
    saveSlot()
    turtle.select(nSlot)
end

function vPrint(str)
    if verbose then print(str) end
end

function refuel()
    turtle.select(baseData.fuelSlot)
    turtle.refuel(1)
    turtle.select(slotSel)
end

function checkFuel()
    if turtle.getFuelLevel() < 1 then
        refuel()
    end
end

function tryLoadGPSData()
    if not fs.exists(gpsDataFile) then return false end
  local navD = tableFromFile(gpsDataFile)
  if navD == nil then return false end
  nav = vectorFromTable(navD[1])
  navDir = navD[2]
  return true
end

function copyTable(it)
    local ot = {}
    for k,v in pairs(it) do
        ot[k] = v
    end
    setmetatable(ot,getmetatable(it))
    return ot
end

function saveGPSData()
    tableToFile(gpsDataFile,{nav,navDir})
end

function saveFuel()
    setRoamingData("fuel",turtle.getFuelLevel())
end

function loadFuel()
    return roamingData.fuel
end

function saveCmdDir(dir)
    setRoamingData("cmdDir",dir)
end

function loadCmdDir()
    return roamingData.cmdDir
end

function getDir()
    return navDir
end

function getReverseDir()
    return (navDir+2)%4
end

function getLoc()
    return copyTable(nav)
end

function setDir(nDir)
    if dir ~= nDir then
        navDir=nDir
        saveGPSData()
    end
end

function translate(vec)
  nav = nav + vec
    saveGPSData()
end

function getVecMod(dir)
  dir = dir or getDir()
  return vecMods[dir]
end

function setSandboxBoundaries(lx,ly,lz,hx,hy,hz)

    an.setBaseData("sandbox_boundaries",{worldToLocal(vector.new(lx,ly,lz)),worldToLocal(vector.new(hx,hy,hz))})
end

function setSandboxAction(action)
    an.setBaseData("sandbox_action",action)
end

function initGPS(useRednet)
    useRednet = useRednet or false
    vPrint("Init GPS...")
    baseData = loadBaseData()
    roamingData = loadRoamingData()
    local sl = loadSlot()
    if sl ~= nil then turtle.select(sl) slotSel = sl end
    if baseData == nil then baseData = {} end
    setBaseData("fuelSlot",15)

  if baseData.navOrigin == nil then setBaseData("navOrigin",vector.new(0,0,0)) end

    if not useRednet then
        vPrint("Using local GPS.")
        if not tryLoadGPSData() then nav = vector.new(0,0,0) navDir=1 vPrint("No file found, resetting.") end
    end

    local fuel = roamingData.fuel
  vPrint("Fuel: "..tostring(fuel))
    if fuel ~= nil then

        vPrint("Loaded fuel:" .. fuel)
        vPrint("TFuel: "..turtle.getFuelLevel())
        if turtle.getFuelLevel() ~= fuel then
            vPrint("Mislocated. Adding one translation.")
            local cmdDir = roamingData.cmdDir
            translate(getVecMod(cmdDir))
            saveFuel()
        end
    end
end

function resetGPS()
    nav = vector.new(0,0,0)
    navDir = 1
    saveFuel()
    saveGPSData()
end

function checkSandbox()
    local sb = an.getBaseData("sandbox_boundaries")
    local sba = an.getBaseData("sandbox_action")

    if sb == nil then return true end
--print("NAV:"..textutils.serialize(nav))
--int("SB:"..textutils.serialize(sb[1]))
    if ( nav.x < sb[1].x or nav.y < sb[1].y or nav.z < sb[1].z ) or ( nav.x > sb[2].x or nav.y > sb[2].y or nav.z > sb[2].z ) then
        if sba == "kill" then error("Terminated due to leaving sandbox constraints.") end
        return false
    end
    return true
end

function forward()
    checkFuel()
    setRoamingData("fuel",turtle.getFuelLevel(),true)
    setRoamingData("cmdDir",getDir())
    local allow = checkSandbox()
    if allow and turtle.forward() then
        setRoamingData("fuel",turtle.getFuelLevel())
        translate(getVecMod())
        return true
    end
    return false
end

function back()
    checkFuel()
    setRoamingData("fuel",turtle.getFuelLevel(),true)
    setRoamingData("cmdDir",getReverseDir())
    local allow = checkSandbox()
    if allow and turtle.back() then
        setRoamingData("fuel",turtle.getFuelLevel())
        translate(getVecMod(getReverseDir()))
        return true
    end
    return false
end

function turnLeft()
    local dir = getDir() - 1
    if dir == 0 then dir = 4 end
    setDir(dir)
    turtle.turnLeft()
end

function turnRight()
    local dir = getDir()
    dir = ( dir + 1 )
    if dir == 5 then dir = 1 end
    setDir(dir)
    turtle.turnRight()
end

function turnDir(nDir)
    if nDir == 5 or nDir == 6 then return end
    cDir = nDir - 1
    local dir = getDir()
    if ( dir ) % 4 == cDir then turnRight()
    else while getDir() ~= nDir do turnLeft() end end
end

function turnAround()
    turnDir(getReverseDir())
end

function up()
    checkFuel()
    setRoamingData("fuel",turtle.getFuelLevel(),true)
    setRoamingData("cmdDir",5)
    local allow = checkSandbox()
    if allow and turtle.up() then
        setRoamingData("fuel",turtle.getFuelLevel())
        translate(getVecMod(5))
        return true
    end
    return false
end

function down()
    checkFuel()
    setRoamingData("fuel",turtle.getFuelLevel(),true)
    setRoamingData("cmdDir",6)
    local allow = checkSandbox()
    if allow and turtle.down() then
        setRoamingData("fuel",turtle.getFuelLevel())
        translate(getVecMod(6))
        return true
    end
    return false
end

function localToWorld(vLocal)
    local vWorld = vector.new(0,0,0)
    vWorld = baseData.navOrigin + vLocal

    return vWorld
end

function worldToLocal(vWorld)
    local vLocal = vector.new(0,0,0)
    vLocal = vWorld - baseData.navOrigin
    return vLocal
end

function vecSum(vecA)
    return vecA.x+vecA.y+vecA.z
end

function mulVec(vecA,vecB)
    return vector.new(vecA.x*vecB.x,vecA.y*vecB.y,vecA.z*vecB.z)
end

function getDirFromVec(vec)
    for i=1,#vecMods,1 do

        if vecSum(vecMods[i]) == vecSum(vec:normalize()) and vec:normalize():sub(vecMods[i]):length() == 0 then return i end
    end
end

function localDelta(vLocal)
    return vLocal - nav
end

function goToWorld(dWorld,mOrder)
    return goTo(an.localDelta(an.worldToLocal(dWorld)),mOrder)
end

function goToLocal(dLocal,mOrder)
    return goTo(an.localDelta(dLocal),mOrder)
end

function goTo(dLocal,mOrder)
    print("moving delta full: "..tostring(dLocal))
    local pIndex = 1
    if dLocal:length() == 0 then return false end
    while #mOrder > 0 do
        local dLocalSub = vector.new(0,0,0)
        local v = mOrder[pIndex]
        dLocalSub[v] = dLocal[v]
        if dLocalSub:length() ~= 0 then
            local tDir = getDirFromVec(dLocalSub)
            if tDir ~= 5 and tDir ~= 6 then
                turnDir(tDir)
            end
            local steps = math.abs(dLocalSub[v])

            local mFunc = forward
            if tDir == 5 then mFunc = up
            elseif tDir == 6 then mFunc = down end
            --print("steps to move: "..steps)
            while steps > 0 and mFunc() do steps = steps-1 end
            local diff = math.abs(dLocalSub[v]) - steps
            --print("modding dLocal by: "..(getVecMod(tDir) * diff)[v])
            dLocal[v] = dLocal[v] - (getVecMod(tDir) * diff)[v]
            --print("moving on "..v)
            --print("moved diff: " .. diff)
            --print("new dLocal: "..dLocal[v])
            --print("new dlocal sub: "..dLocalSub[v])
            --read()
            if diff == math.abs(dLocalSub[v]) then
                --print("move on this axis done")
                --read()
                table.remove(mOrder,pIndex)
            else
                --print("moving to next axis to come bakc later")
                --read()
                pIndex = pIndex + 1
            end

            if pIndex > #mOrder then pIndex = 1 end
        else
            table.remove(mOrder,pIndex)
            --print("removed "..v.." From move order (delta==0)") read()
            if pIndex > #mOrder then pIndex = 1 end
        end
    end
end

function test(ip)
    for i=1,ip,1 do
        print(getLoc())
        goTo(vector.new(2,0,1),{"z","x"})
        goTo(vector.new(0,0,-1),{"z"})
        print(getLoc())
        goTo(vector.new(-2,0,-1),{"z","x"})
        goTo(vector.new(0,0,1),{"z"})
    end
end