C++ code for A* search