Implement heap sort correctly