fly_to

function correctfacing()
                if (facingnum == 5) then
                        facingnum = 1
                        elseif (facingnum == 0) then
                        facingnum = 4
                end
end

function facedir(dir)
if (dir == "n") then
dirnum = 1
elseif (dir == "e") then
dirnum = 2
elseif (dir == "s") then
dirnum = 3
elseif (dir == "w") then
dirnum = 4
end
facingprocess = 1
while (facingprocess == 1) do
                if (facingnum < dirnum) then
                        turtle.turnRight()
                        facingnum = (facingnum + 1)
                        elseif (facingnum > dirnum) then
                        turtle.turnLeft()
                        facingnum = (facingnum - 1)
                        elseif (facingnum == dirnum) then
                        facingprocess = 0
                end
        correctfacing()
end
end


function tforward()
        turtle.dig()
        turtle.forward()
        if (facingnum == 1) then
        z = (z - 1)
        elseif (facingnum == 2) then
        x = (x + 1)
        elseif (facingnum == 3) then
        z = (z + 1)
        elseif (facingnum == 4) then
        x = (x - 1)
        end
end

function tup()
        turtle.digUp()
        turtle.up()
        y = (y+1)
end

function tdown()
        turtle.digDown()
        turtle.down()
        y = (y-1)
end

function tright()
turtle.turnRight()
facingnum = (facingnum + 1)
correctfacing()
end

function tleft()
turtle.turnLeft()
facingnum = (facingnum - 1)
correctfacing()
end

function flyto(tarx,tary,tarz)
tx = tonumber(tarx)
ty = tonumber(tary)
tz = tonumber(tarz)
fly = 1
flyingloop()
end

function flyingloop()
        while (fly == 1) do
                if (tonumber(y) < tonumber(ty)) then
                tup()
                elseif (tonumber(z) > tonumber(tz)) then
                facedir("n")
                tforward()
                elseif (tonumber(z) < tonumber(tz)) then
                facedir("s")
                tforward()
                elseif (tonumber(x) < tonumber(tx)) then
                facedir("e")
                tforward()
                elseif (tonumber(x) > tonumber(tx)) then
                facedir("w")
                tforward()
                elseif (tonumber(y) > tonumber(ty)) then
                tdown()
                else
                fly = 0
                end
        end
end

function setcoords()
term.clear()
term.setCursorPos(1,1)
print("Please input target coordinates")
write("X: ")
targetx = read()
write("Y: ")
targety = read()
write("Z: ")
targetz = read()
print("Coordinates set.")
sleep(2)
end

function status()
term.clear()
term.setCursorPos(1,1)
if (facingnum == 1) then
facing = "n"
elseif (facingnum == 2) then
facing = "e"
elseif (facingnum == 3) then
facing = "s"
elseif (facingnum == 4) then
facing = "w"
end
print("X: "..x.." Y: "..y.." Z: "..z.." Facing: "..facing)
sleep(4)
end


function getOrientation()
loc1 = vector.new(gps.locate(2, false))
if not turtle.forward() then
    for j=1,6 do
            if not turtle.forward() then
                    turtle.dig()
         else break end
    end
end
loc2 = vector.new(gps.locate(2, false))
heading = loc2 - loc1
return ((heading.x + math.abs(heading.x) * 2) + (heading.z + math.abs(heading.z) * 3))
end


function calibrate()
print("Please input turtles current X, Y and Z coordinates.")
x,y,z = gps.locate()
--write("X: ")
--tempx = read()
--x = tonumber(tempx)
--write("Y: ")
--tempy = read()
--y = tonumber(tempy)
--write("Z: ")
--tempz = read()
--z = tonumber(tempz)
print("")
print("Coordinates set.")
print("Please input the facing of the turtle, n / e / s / w")
-- write("Facing: ")
-- facingloop = 1
-- while (facingloop == 1) do
-- facing = read()
-- if (facing == "n") then
-- facingnum = 1
-- facingloop = 0
-- elseif (facing == "e") then
-- facingloop = 0
-- facingnum = 2
-- elseif (facing == "s") then
-- facingloop = 0
-- facingnum = 3
-- elseif (facing == "w") then
-- facingloop = 0
-- facingnum = 4
-- else
-- print("Invalid input")
-- end
-- end
facingnum = getOrientation()
print("Facing set. " .. facingnum)
sleep(1)
--main()
flyto(331,271,80)
end