Add initial code for A* search