Improving priority queue, still some bugs
It works!