Implement heap sort