Add priority queue implemented by binary heap

Add priority queue implemented by binary heap. Will replace the priority
queue  with the fake one used before.