Added PriorityQueue for better A* performance