Finished A* implementation