Implemented heap sort and tests