Помогите с реализацией алгоритма сортировки деревом. Нужен код на C++

#include <iterator>

template< typename Iterator >
void adjust_heap( Iterator first
                 , typename std::iterator_traits< Iterator >::difference_type current
                 , typename std::iterator_traits< Iterator >::difference_type size
                 , typename std::iterator_traits< Iterator >::value_type tmp )
{
   typedef typename std::iterator_traits< Iterator >::difference_type diff_t;

   diff_t top = current, next = 2 * current + 2;

   for ( ; next < size; current = next, next = 2 * next + 2 )
   {
       if ( *(first + next) < *(first + next - 1) )
           --next;
       *(first + current) = *(first + next);
   }

   if ( next == size )
       *(first + current) = *(first + size - 1), current = size - 1;

   for ( next = (current - 1) / 2;
         top < current && *(first + next) < tmp;
         current = next, next = (current - 1) / 2 )
   {
       *(first + current) = *(first + next);
   }
   *(first + current) = tmp;
}

template< typename Iterator >
void pop_heap( Iterator first, Iterator last)
{
   typedef typename std::iterator_traits< Iterator >::value_type value_t;

   value_t tmp = *--last;
   *last = *first;
   adjust_heap( first, 0, last - first, tmp );
}

template< typename Iterator >
void heap_sort( Iterator first, Iterator last )
{
   typedef typename std::iterator_traits< Iterator >::difference_type diff_t;
   for ( diff_t current = (last - first) / 2 - 1; current >= 0; --current )
       adjust_heap( first, current, last - first, *(first + current) );

   while ( first < last )
       pop_heap( first, last-- );
}

RDC2mail, +1 за простой код :)