First working version of a heap based priority queue