A* search nearly working.