Add A* search algorithm