Added aStarSearch (not tested)