Optimize Heap Sort for better speed 1