Find shortest path using BFS | MathWorks