--TurtleGPS
--load it with os.loadAPI
--INFO: for calibration, in front of the turtle must be air to recognize direction

timeout=5

function setDefaultTimeout(to)
    timeout=to
end

dir=0
posX=0
posY=0
posZ=0

function fixDir()
    if(dir<0) then
        dir=dir+4
    end
    if(dir>3) then
        dir=dir-4
    end
end

function mov(x,z)
    if(dir==0) then
        posZ=posZ-z
        posX=posX+x
    elseif(dir==2) then
        posZ=posZ+z
        posX=posX-x
    elseif(dir==1) then
        posZ=posZ+x
        posX=posX+z
    elseif(dir==2) then
        posZ=posZ-x
        posX=posX-z
    end
end

function getPos()
    return getPos(timeout)
end

verbosity=true

function setSilence(silence)
    verbosity=(not silence)
end

function getPos(tout)
    to=timeout
    if(tout) then
        to=tout
    end
    gx,gy,gz=gps.qwgps_orig_locate(tout)
    if((gx~=nil and (not(x ~= x)))and((gx~=posX)or(gy~=posY)or(gz~=posZ))) then
        if verbosity then
            print("[qwgps] INFO local coords don't equal with remote coords: LocalX"..tostring(posX).."Y"..tostring(posY).."Z"..tostring(posZ).." SatX"..tostring(gx).."Y"..tostring(gy).."Z"..tostring(gz))
        end
        posX=gx
        posY=gy
        posZ=gz
        pcall(writeCD)
    end
    
    return posX,posY,posZ
end

function getPosNoScan()
    return posX,posY,posZ
end

function init()
    turtle.qwgps_orig_back=turtle.back
    turtle.back=function()
        ok=turtle.qwgps_orig_back()
        if ok then
            mov(0,-1)
            pcall(writeCD)
        end
        return ok
    end
    turtle.qwgps_orig_forward=turtle.forward
    turtle.forward=function()
        ok=turtle.qwgps_orig_forward()
        if ok then
            mov(0,1)
            pcall(writeCD)
        end
        return ok
    end
    turtle.qwgps_orig_goLeft=turtle.left
    turtle.left=function()
        ok=turtle.qwgps_orig_goLeft()
        if ok then
            mov(-1,0)
            pcall(writeCD)
        end
        return ok
    end
    turtle.qwgps_orig_goRight=turtle.right
    turtle.right=function()
        ok=turtle.qwgps_orig_goRight()
        if ok then
            mov(1,0)
            pcall(writeCD)
        end
        return ok
    end
    turtle.qwgps_orig_up=turtle.up
    turtle.up=function()
        ok=turtle.qwgps_orig_up()
        if ok then
            posY=posY+1
            pcall(writeCD)
        end
        return ok
    end
    turtle.qwgps_orig_down=turtle.down
    turtle.down=function()
        ok=turtle.qwgps_orig_down()
        if ok then
            posY=posY-1
            pcall(writeCD)
        end
        return ok
    end
    turtle.qwgps_orig_turnLeft=turtle.turnLeft
    turtle.turnLeft=function()
        ok=turtle.qwgps_orig_turnLeft()
        if ok then
            dir=dir-1
            pcall(writeCD)
        end
        return ok
    end
    turtle.qwgps_orig_turnRight=turtle.turnRight
    turtle.turnRight=function()
        ok=turtle.qwgps_orig_turnRight()
        if ok then
            dir=dir+1
            pcall(writeCD)
        end
        return ok
    end
    gps.qwgps_orig_locate=gps.locate
end

function overrideGpsDotLocate()
    gps.locate=function(tout,debug)
        return getPos(tout)
    end
end

function restoreAPIs()
    turtle.forward=turtle.qwgps_orig_forward
    turtle.back=turtle.qwgps_orig_back
    turtle.left=turtle.qwgps_orig_goLeft
    turtle.right=turtle.qwgps_orig_goRight
    turtle.up=turtle.qwgps_orig_up
    turtle.down=turtle.qwgps_orig_down
    turtle.turnLeft=turtle.qwgps_orig_turnLeft
    turtle.turnRight=turtle.qwgps_orig_turnRight
    gps.locate=gps.qwgps_orig_locate
end

function calib_scan()
    fx,fy,fz=gps.qwgps_orig_locate(timeout)
    if(fx==nil) then
        print("Enter turtle's current Coords: ")
        term.write("X")
        fx=tonumber(read())
        term.write("Y")
        fy=tonumber(read())
        term.write("Z")
        fz=tonumber(read())
    end
    return fx,fy,fz
end
init()
overrideGpsDotLocate()
function readCD()
    if(fs.exists("qwgps_coords")) then
        h=fs.open("qwgps_coords","r")
        dir=tonumber(h.readLine())
        posX=tonumber(h.readLine())
        posY=tonumber(h.readLine())
        posZ=tonumber(h.readLine())
        h.close()
        return true
    end
    return false
end
function writeCD()
    h=fs.open("qwgps_coords","w")
    h.writeLine(tostring(dir))
    h.writeLine(tostring(posX))
    h.writeLine(tostring(posY))
    h.writeLine(tostring(posZ))
    h.flush()
    h.close()
end
function calibrate()
    ax,ay,az=calib_scan()
    while not turtle.qwgps_orig_forward() do
        print("[qwgps] In front of the turtle must be air")
        sleep(4)
    end
    bx,by,bz=calib_scan()
    while not turtle.qwgps_orig_back() do
        print("[qwgps] On back side of the turtle must be air")
        sleep(4)
    end
    if(bx>ax) then
        dir=1
    elseif(bx<ax) then
        dir=3
    elseif(bz>az) then
        dir=2
    elseif(bz<az) then
        dir=0
    end
    posX=ax
    posY=ay
    posZ=az
end
if not readCD() then
    calibrate()
    writeCD()
end