Go to location

function setHeading(heading,newHeading)
     if(heading ~= 'N' and heading ~= 'n' and heading ~= 'E' and heading ~= 'e' and heading ~= 'S' and heading ~= 's' and heading ~= 'W' and heading ~= 'w') then
        if(turtle.getFuelLevel() < 2) then
            error("Insufficient fuel")
        end
                local start = vector.new(gps.locate())
                while(turtle.detect()) do
                        turtle.dig()
                end
                turtle.forward()
                local current = vector.new(gps.locate())
                turtle.back()
                if(start.z - current.z > 0) then
                        heading = 'N'
                elseif (start.z - current.z < 0) then
                        heading = 'S'
                end
                if(start.x - current.x > 0) then
                        heading = 'W'
                elseif (start.x - current.x < 0) then
                        heading = 'E'
                end
        end

    if(heading ~= newHeading) then
        if(newHeading == 'N' or newHeading == 'n') then
            if(heading == 'S' or heading == 's') then
                turtle.turnLeft()
                turtle.turnLeft()
            end
            if(heading == 'E' or heading == 'e') then
                turtle.turnLeft()
            end
            if(heading == 'W' or heading == 'w') then
                turtle.turnRight()
            end
        end
        if(newHeading == 'E' or newHeading == 'e') then
            if(heading == 'S' or heading == 's') then
                turtle.turnLeft()
            end
            if(heading == 'N' or heading == 'n') then
                turtle.turnRight()
            end
            if(heading == 'W' or heading == 'w') then
                turtle.turnRight()
                turtle.turnRight()
            end
        end
        if(newHeading == 'S' or newHeading == 's') then
            if(heading == 'N' or heading == 'n') then
                turtle.turnLeft()
                turtle.turnLeft()
            end
            if(heading == 'E' or heading == 'e') then
                turtle.turnRight()
            end
            if(heading == 'W' or heading == 'w') then
                turtle.turnLeft()
            end
        end
        if(newHeading == 'W' or newHeading == 'w') then
            if(heading == 'S' or heading == 's') then
                turtle.turnRight()
            end
            if(heading == 'E' or heading == 'e') then
                turtle.turnLeft()
                turtle.turnLeft()
            end
            if(heading == 'N' or heading == 'n') then
                turtle.turnLeft()
            end
        end
    end
end

function getHeading()
    if(turtle.getFuelLevel() < 2) then
        error("Insufficient fuel")
    end
    local start = vector.new(gps.locate())
    while(turtle.detect()) do
        turtle.dig()
    end
    turtle.forward()
    local current = vector.new(gps.locate())
    turtle.back()
    if(start.z - current.z > 0) then
        heading = 'N'
    elseif (start.z - current.z < 0) then
        heading = 'S'
    end
    if(start.x - current.x > 0) then
        heading = 'W'
    elseif (start.x - current.x < 0) then
        heading = 'E'
    end
        return heading
end

function goToLocation(location,heading)
    if(heading ~= 'N' and heading ~= 'n' and heading ~= 'E' and heading ~= 'e' and heading ~= 'S' and heading ~= 's' and heading ~= 'W' and heading ~= 'w') then
        heading = getHeading()
    end

    local current = vector.new(gps.locate())
    if(location.x ~= current.x or location.y ~= current.y or location.z ~= current.z) then
        if(turtle.getFuelLevel() < 2) then
            error("Insufficient fuel")
        end
    end
    while(location.y ~= current.y) do
        if(location.y > current.y) then
            while(turtle.detectUp()) do
                turtle.digUp()
            end
            turtle.up()
        else
            while(turtle.detectDown()) do
                turtle.digDown()
            end
            turtle.down()
        end
        current = vector.new(gps.locate())
    end

    while(location.z ~= current.z) do
        if(location.z > current.z) then
            setHeading(heading,'S')
            heading = 'S'
        elseif(location.z < current.z) then
            setHeading(heading,'N')
            heading = 'N'
        end
        while(turtle.detect()) do
            turtle.dig()
        end
        turtle.forward()
        current = vector.new(gps.locate())
    end

    while(location.x ~= current.x) do
        if(location.x > current.x) then
            setHeading(heading,'E')
            heading = 'E'
        elseif(location.x < current.x) then
            setHeading(heading,'W')
            heading = 'W'
        end
        while(turtle.detect()) do
            turtle.dig()
        end
        turtle.forward()
        current = vector.new(gps.locate())
    end
    return heading
end

print("X: ")
x = tonumber(read())
print("Y: ")
y = tonumber(read())
print("Z: ")
z = tonumber(read())
print("Heading: ")
heading = read()

location = vector.new(x,y,z)


print(goToLocation(location,heading))