Fixed A* search logic; added PriorityQueue to lib