Implement heap sort algorithm