AStar algorithm added, bugs may appear