go_to

-- The way the face of the turtle is oriented
-- N = 0; E = 1; S = 2; W = 3
if #arg == 4 then
    home_cords = vector.new(arg[1], arg[2], arg[3])
    turtle_face = arg[4]
end

local pos = vector.new(gps.locate())
local diff = pos:sub(home_cords)

-- Helper functions
local function around()
    turtle.turnLeft()
    turtle.turnLeft()
    if turtle_face + 2 == 4 then
        turtle_face = 0
    elseif turtle_face + 2 == 5 then
        turtle_face = 1
    else
        turtle_face = turtle_face + 2
    end
end

local function Left()
    turtle.turnLeft()
    if turtle_face - 1 == -1 then
        turtle_face = 3
    else
        turtle_face = turtle_face - 1
    end
end

local function Right()
    turtle.turnRight()
    if turtle_face + 1 == 4 then
        turtle_face = 0
    else
        turtle_face = turtle_face + 1
    end
end

local function update(cords)
    pos = vector.new(gps.locate())
    diff = pos:sub(cords)
end

local function check_stuck()
    while turtle_face ~= 0 do
        Right()
    end

    for x=0,3 do
        if turtle.detect() then
            Left()
        else
            turtle.forward()
        end
    end
end

-- Main function
local function return_to(cords)
    --- Cord is a table, from the vector API, in {x='x', y='y', z='z'}
    while diff.x ~= 0 or diff.y ~= 0 or diff.z ~= 0 do
        pos = vector.new(gps.locate())

        -- For the x-field
        if diff.x < 0 then
            print('diff x < 0')
            if turtle_face ~= 1 then
                while turtle_face ~= 1 do
                    Right()
                    print('face (if) : ', turtle_face)
                end
            end

            while not turtle.detect() and diff.x < 0 and turtle_face == 1 do
                print('diff neg x: ', diff)
                print('pos: ', pos)
                print('face: ', turtle_face)
                turtle.forward()
                update(cords)
            end

        elseif diff.x > 0 then
            print('diff x > 0')
            if turtle_face ~= 3 then
                while turtle_face ~= 3 do
                    Right()
                    print('face (if) : ', turtle_face)
                end
            end

            while not turtle.detect() and diff.x > 0 and turtle_face == 3 do
                print('diff pos x: ', diff)
                print('pos: ', pos)
                print('face: ', turtle_face)
                turtle.forward()
                update(cords)
            end
        end

        -- For the y-field
        if diff.y < 0 then
            while not turtle.detectUp() and diff.y < 0 do
                print('diff neg y: ', diff)
                print('pos: ', pos)
                print('face: ', turtle_face)
                turtle.up()
                update(cords)
            end
        elseif diff.y > 0 then
            while not turtle.detectDown() and diff.y > 0 do
                print('diff pos y: ', diff)
                print('pos: ', pos)
                print('face: ', turtle_face)
                turtle.down()
                update(cords)
            end
        end

        -- For the z-field
        if diff.z < 0 then
            print('diff z < 0')
            if turtle_face ~= 0 then
                while turtle_face ~= 0 do
                    Right()
                    print('face (if) : ', turtle_face)
                end
            end

            while not turtle.detect() and diff.z < 0 and turtle_face == 0 do
                print('diff neg y: ', diff)
                print('pos: ', pos)
                print('face: ', turtle_face)
                turtle.forward()
                update(cords)
            end
        elseif diff.z > 0 then
            print('diff z > 0')
            if turtle_face ~= 2 then
                while turtle_face ~= 2 do
                    Right()
                    print('face (if) : ', turtle_face)
                end
            end

            while not turtle.detect() and diff.z > 0 and turtle_face == 2 do
                print('diff neg y: ', diff)
                print('pos: ', pos)
                print('face: ', turtle_face)
                turtle.forward()
                update(cords)
            end

        end

    end
end

return_to(home_cords)