Add heap sort with unit tests