Add priority queue implementation & unit tests