C++ implementation of solving a maze and find a solution despite of the obstacles