Solve 006-maze-shortest-path in c++