Add heap sort implementation