Priority queue implementation and more skeleton for pathfinder