Improve nth_element performance and unit testing