//
// Created by joe on 1/14/17.
//

#include "mqsort.h"

template<typename T>
inline void swap(T &a, T &b) {
  T t = a;
  a = b;
  b = t;
}

template<typename T>
inline void insert(T *l, T *r, int (*compar)(const void *, const void *)) {
  T *s = l;
  while (s < r) {
    T *k = s + 1;
    while (k > l && compar(k, k - 1) < 0) {
      swap(*k, *(k - 1));
      --k;
    }
    ++s;
  }
}

#define INS 12
#define MAXS 100
#define PUSH(x, y) {stack[top][0] = x; stack[top][1] = y; ++top; }
#define POP(x, y) {--top; x = stack[top][0]; y = stack[top][1];}
#define EMPTY (top == 0)

template<typename T>
void secret1(T *l, T *r, int (*compar)(const void *, const void *)) {
  int top = 0;
  T *stack[MAXS][2];
  while (true) {
    if (l + INS > r) {
      insert(l, r, compar);
      if (EMPTY)
        break;
      POP(l, r);
      continue;
    }
    T *j = r;
    T *i = l + 1;
    {
      int a = compar(l, l + 1), b = compar(l, l + 2), c = compar(l + 1, l + 2);
      if (a > 0) {
        if (b > 0)
          if (c < 0) swap(*l, l[2]);
          else swap(*l, l[1]);
      } else if (a < 0) {
        if (b < 0)
          if (c < 0) swap(*l, l[1]);
          else swap(*l, l[2]);
      }
    }
    while (i <= j) {
      int a = compar(l, i);
      if (a <= 0) {
        int b = 0;
        while (j > l && (b = compar(l, j)) < 0)
          --j;
        if (j > i) {
          if (a != b) {
            swap(*j, *i);
            --j;
          } else {
            ++i;
            --j;
          }
        } else break;
      } else {
        ++i;
      }
    }
    swap(*l, *j);
    --j;
    if (j > l) {
      if (r > i) {
        if (j - l > r - i) {
          PUSH(l, j);
          l = i;
        } else {
          PUSH(i, r);
          r = j;
        }
      } else r = j;
    } else if (r > i) {
      l = i;
    } else if (EMPTY)
      break;
    else POP(l, r);
  }
}

inline void swapsize(char *a, char *b, size_t size) {
  for (int cnt = 0; cnt < size; ++cnt)
    swap<char>(a[cnt], b[cnt]);
}

void secret(char *l, char *r, size_t size, int (*compar)(const void *, const void *)) {
  char *j = r;
  char *i = l + size;
  while (i <= j) {
    if (compar(l, i) < 0) {
      swapsize(j, i, size);
      j -= size;
    } else {
      i += size;
    }
  }
  swapsize(l, j, size);
  j -= size;
  if (l < j)
    secret1(l, j, compar);
  if (i < r)
    secret1(i, r, compar);

}

void mqsort(void *base, size_t num, size_t size, int (*compar)(const void *, const void *)) {
  switch (size) {
    case 1:
      secret1((uint8_t *) base, (uint8_t *) base + (num - 1), compar);
      break;
    case 2:
      secret1((uint16_t *) base, (uint16_t *) base + (num - 1), compar);
      break;
    case 4:
      secret1((uint32_t *) base, (uint32_t *) base + (num - 1), compar);
      break;
    case 8:
      secret1((uint64_t *) base, (uint64_t *) base + (num - 1), compar);
      break;
    default:
      secret((char *) base, (char *) base + (num - 1) * size, size, compar);
  }
}