moveto

x = tonumber(arg[1])
y = tonumber(arg[2])
z = tonumber(arg[3])

int_x, int_y, int_z = gps.locate()

function updateCords()
    int_x, int_y, int_z = gps.locate()
    return int_x, int_y, int_z

end

function facingWhere(facing)
    print("Figuring out where I am facing")

    int_x, int_y, int_z = updateCords()

    if not turtle.detect() then
        turtle.forward()
    else
        turtle.dig()
        turtle.forward()
    end

    print("Getting coordinates..")
    cmp_x, cmp_y, cmp_z = gps.locate()
    print("I am at: "..cmp_x.." "..cmp_y.." "..cmp_z)

    if (int_x - cmp_x == 0) == false then
        if (int_x - cmp_x == -1) then
                facing = "east"
        elseif (int_x - cmp_x == 1) then
                facing = "west"
        end
    elseif (int_z - cmp_z == 0) == false then
        if (int_z - cmp_z == -1) then
                facing = "south"
        elseif (int_z - cmp_z == 1) then
                facing = "north"
        end
    end

    return facing

end

function face(dir, facing)
    --facing = facingWhere()

    print("Turnig to face "..dir..". I am facing "..tostring(facing))

    if dir == facing then
        return facing
    end

    if facing == "north" then
        if dir == "west" then
            turtle.turnLeft()
            facing = "west"
            return facing
        elseif dir == "east" then
            turtle.turnRight()
            facing = "east"
            return facing
        elseif dir == "south" then
            turtle.turnRight()
            turtle.turnRight()
            facing = "south"
            return facing
        end
    elseif facing == "west" then
        if dir == "north" then
            turtle.turnRight()
            facing = "north"
            return facing
        elseif dir == "south" then
            turtle.turnLeft()
            facing = "south"
            return facing
        elseif dir == "east" then
            turtle.turnRight()
            turtle.turnRight()
            facing = "east"
            return facing
        end
    elseif facing == "south" then
        if dir == "west" then
            turtle.turnRight()
            facing = "west"
            return facing
        elseif dir == "east" then
            turtle.turnLeft()
            facing = "east"
            return facing
        elseif dir == "north" then
            turtle.turnRight()
            turtle.turnRight()
            facing = "north"
            return facing
        end
    elseif facing == "east" then
        if dir == "north" then
            turtle.turnLeft()
            facing = "north"
            return facing
        elseif dir == "south" then
            turtle.turnRight()
            facing = "south"
            return facing
        elseif dir == "west" then
            turtle.turnRight()
            turtle.turnRight()
            facing = "west"
            return facing
        end
    end

    if facing == nil then
        facing = facingWhere(facing)
        face(dir, facing)
    end

    if (facing == dir) == false then
        go(dir)
    end
end

function go(direction, coord)
    int_x, int_y, int_z = updateCords()
    print("GOING")

    facing = face(direction, facing)

    if coord == "z" then
        while (int_z == z) == false do
            if direction == facing then
                if turtle.detect() then
                    turtle.dig()
                    turtle.forward()
                else
                    turtle.forward()
                end
            end
            int_x, int_y, int_z = updateCords()
        end
    elseif coord == "x" then
        while (int_x == x) == false do
            if direction == facing then
                if turtle.detect() then
                    turtle.dig()
                    turtle.forward()
                else
                    turtle.forward()
                end
            end
            int_x, int_y, int_z = updateCords()
        end
    end
end

function moveTo(x, y, z)
    facing = facingWhere(facing)
    z_want = ""
    x_want = ""
    print("Target: "..x.." "..y.." "..z)

    if x == int_x and y == int_y and z == int_z then
        print("Already at destination.")
        return
    end


    while true do
        int_x, int_y, int_z = updateCords()

        if z > int_z then
            z_want = "south"
        elseif z < int_z then
            z_want = "north"
        end

        if x > int_x then
            x_want = "east"
        elseif x < int_x then
            x_want = "west"
        end

        if (z == int_z) == false then
            go(z_want, "z")
        elseif (x == int_x) == false then
            go(x_want, "x")
        end


        int_x, int_y, int_z = updateCords()

        if x == int_x and z == int_z then
            print("Arrived at destination.")
            break
        end
    end
end

moveTo(x, y, z)