Tried to implement C++'s STL Sort routine -- Introsort

introsort.cpp

/* Trying to implement Introsort for integer array
   Used in STL
*/

/* Headers for testing */
#include <cstdlib>
#include <iostream>

namespace IntroSort {

namespace introsort_internals {

template <typename T>
bool lessThan(T a, T b) {
  return (a < b);
}
template <typename T>
bool greaterThan(T a, T b) {
  return (a > b);
}

template <typename T>
void _swap_(T *a, T *b) {
  T temp = *(a);
  *(a) = *(b);
  *(b) = temp;
}

template <typename T>
class COMP {
 private:
  T comp;

 public:
  COMP(T x) : comp(x) {}
  template <typename T1>
  bool operator()(T1 *a, T1 *b) {
    return bool(comp(*a, *b));
  }
};

template <typename T>
COMP<T> __middleware_(T x) {
  return COMP<T>(x);
}

template <typename T, typename T2>
void _heapify_(T *x, T *begin, T *end, COMP<T2> _comp) {
  size_t largest, cur, left, right;
  size_t total = end - begin;
  cur = x - begin;
  while (true) {
    largest = cur;
    left = 2 * largest + 1;
    right = 2 * largest + 2;
    if (left < total && _comp(begin + largest, begin + left)) {
      largest = left;
    }
    if (right < total && _comp(begin + largest, begin + right)) {
      largest = right;
    }
    if (largest != cur) {
      _swap_(begin + largest, begin + cur);
      cur = largest;
    } else {
      break;
    }
  }
}

template <typename T, typename T2>
void _heapSort_(T *begin, T *end, COMP<T2> _comp) {
  T *x = begin + (end - begin) / 2 - 1;
  while (x - begin >= 0) {
    _heapify_(x, begin, end, _comp);
    x--;
  }
  x = begin + (end - begin) - 1;
  while (x - begin > 0) {
    _swap_(x, begin);
    _heapify_(begin, begin, x, _comp);
    x--;
  }
  return;
}

template <typename T, typename T2>
void _insertionSort_(T *begin, T *end, COMP<T2> _comp) {
  if (end - begin <= 1) {
    return;
  }
  size_t size = end - begin;
  size_t i;
  size_t j;
  T temp;
  for (i = 1; i < size; i++) {
    j = i - 1;
    temp = *(begin + i);
    while (j >= 1 && _comp(&temp, begin + j)) {
      *(begin + j + 1) = *(begin + j);
      j--;
    }
    if (j == 0 && _comp(&temp, begin)) {
      T temp2 = *begin;
      *begin = temp;
      temp = temp2;
    }
    *(begin + j + 1) = temp;
  }
  return;
}

template <typename T, typename T2>
T *_findMedain_(T *a, T *b, T *c, COMP<T2> _comp) {
  if (_comp(a, b) && _comp(b, c)) {
    return b;
  } else if (_comp(b, a) && _comp(a, c)) {
    return a;
  } else {
    return c;
  }
}

template <typename T, typename T2>
T *_partition_(T *begin, T *end, T *pivot, COMP<T2> _comp) {
  _swap_(pivot, begin);
  pivot = begin;
  T *first = begin;
  T *last = end;
  while (true) {
    first++;
    while (_comp(first, pivot) && first != end) {
      first++;
    }
    last--;
    while (_comp(pivot, last) && last != pivot) {
      last--;
    }
    if (first >= last) {
      break;
    }
    _swap_(first, last);
  }
  _swap_(begin, last);
  return last;
}

template <typename T, typename T2>
void _introSort_(T *begin, T *end, size_t depth, COMP<T2> _comp) {
  size_t size = end - begin;
  if (size <= 16) {
    _insertionSort_(begin, end, _comp);
    return;
  } else if (depth == 0) {
    _heapSort_(begin, end, _comp);
    return;
  }
  T *pivot = _findMedain_(begin, begin + (end - begin) / 2, end, _comp);
  pivot = _partition_(begin, end, pivot, _comp);
  _introSort_(begin, pivot, depth - 1, _comp);
  _introSort_(pivot + 1, end, depth - 1, _comp);
}

}  // namespace introsort_internals

template <typename T>
void sort(T *begin, T *end) {
  size_t x = begin - end;
  size_t maxDepth = 0;
  while (x) {
    x /= 2;
    maxDepth++;
  }
  IntroSort::introsort_internals::_introSort_(
      begin, end, maxDepth,
      IntroSort::introsort_internals::__middleware_(
          IntroSort::introsort_internals::lessThan<T>));
  return;
}

template <typename T, typename T2>
void sort(T *begin, T *end, T2 _comp) {
  size_t x = begin - end;
  size_t maxDepth = 0;
  while (x & 1) {
    x /= 2;
    maxDepth++;
  }
  IntroSort::introsort_internals::_introSort_(
      begin, end, maxDepth,
      IntroSort::introsort_internals::__middleware_(_comp));
}

}  // namespace IntroSort

int main() {
  int n;
  std::cin >> n;
  int arr[n];
  for (int i = 0; i < n; i++) {
    arr[i] = rand() % 1000 + 1;
  }
  IntroSort::sort(arr + 0, arr + n, std::greater<int>());
  for (int i = 0; i < n; i++) {
    std::cout << arr[i] << " ";
  }
  std::cout << "\n";
  return 0;
}