Make PriorityQueue a class and add tests