Heapsort implemented on the pathfinding algorithm