A* pathfinder in Cpp