directions = {"north", "east", "south", "west"}

local position = {}
local direction = ""
function init(x, y, z, dir)
    position = to_pos(x, y, z)
    direction = dir
end

function forward()
    local val = turtle.forward()
    if val then
        forward_increment()
        print_pos()
    end
    return val
end

function back()
    local val = turtle.back()
    if val then
        backward_increment()
        print_pos()
    end
    return val
end

function up()
    local val = turtle.up()
    if val then
        position = {
            x = position.x,
            y = position.y + 1,
            z = position.z
        }
        print_pos()
    end
    return val
end

function down()
    local val = turtle.down()
    if val then
        position = {
            x = position.x,
            y = position.y - 1,
            z = position.z
        }
        print_pos()
    end
    return val
end

function turnLeft()
    local val = turtle.turnLeft()
    if val then
        if direction == "north" then
            direction = "west"
        elseif direction == "east" then
            direction = "north"
        elseif direction == "south" then
            direction = "east"
        elseif direction == "west" then
            direction = "south"
        end
    end
    return val
end

function turnRight()
    local val = turtle.turnRight()
    if val then
        if direction == "north" then
            direction = "east"
        elseif direction == "east" then
            direction = "south"
        elseif direction == "south" then
            direction = "west"
        elseif direction == "west" then
            direction = "north"
        end
    end
    return val
end
function forward_increment()
    if direction == directions[1] then
        position = {
            x = position.x,
            y = position.y,
            z = position.z - 1
        }
    elseif direction == directions[2] then
        position = {
            x = position.x + 1,
            y = position.y,
            z = position.z
        }
    elseif direction == directions[3] then
        position = {
            x = position.x,
            y = position.y,
            z = position.z + 1
        }
    elseif direction == directions[4] then
        position = {
            x = position.x - 1,
            y = position.y,
            z = position.z
        }
    end
end

function backward_increment()
    if direction == directions[1] then
        position = {
            x = position.x,
            y = position.y,
            z = position.z + 1
        }
    elseif direction == directions[2] then
        position = {
            x = position.x - 1,
            y = position.y,
            z = position.z
        }
    elseif direction == directions[3] then
        position = {
            x = position.x,
            y = position.y,
            z = position.z - 1
        }
    elseif direction == directions[4] then
        position = {
            x = position.x + 1,
            y = position.y,
            z = position.z
        }
    end
end

function to_pos(x1, y1, z1)
    return {x=x1, y=y1, z=z1}
end

function print_pos()
    print("Position: (" .. position.x .. ", " .. position.y .. ", " .. position.z .. ")")
end

function turn_to(x, z)
    if x == 1 then
        while direction ~= "east" do
            turnRight()
        end
    elseif x == -1 then
        while direction ~= "west" do
            turnRight()
        end
    elseif z == 1 then
        while direction ~= "south" do
            turnRight()
        end
    elseif z == -1 then
        while direction ~= "north" do
            turnRight()
        end
    end
end

function goto(xPos, yPos, zPos)
    -- Get direction vector
    local xDir, yDir, zDir
    if xPos > position.x then
        xDir = 1
    else
        xDir = -1
    end
    if yPos > position.y then
        yDir = 1
    else
        yDir = -1
    end
    if zPos > position.z then
        zDir = 1
    else
        zDir = -1
    end

    -- match x
    turn_to(xDir, 0)
    while (position.x ~= xPos) do
        if turtle.detect() then
            turtle.dig()
        end
        forward()
    end

    -- match z
    turn_to(0, zDir)
    while (position.z ~= zPos) do
        if turtle.detect() then
            turtle.dig()
        end
        forward()
    end

    -- match y
    while (position.y ~= yPos) do
        if yDir == 1 then
            if turtle.detectUp() then
                turtle.digUp()
            end
            up()
        else
            if turtle.detectDown() then
                turtle.digDown()
            end
            down()
        end
    end
end