Got improved heap sort working