A* implementation in C++