#pragma once

#include <types.h>

template <typename T>
constexpr void swap(T& l, T& r) {
  const T t = r;
  r = l;
  l = t;
}

/**
 * Quicksort median-of-three pivot
 */
template <typename T>
constexpr void qsort_pivot(T a[], const size_t begin, const size_t end) {
  const auto mid = (begin + end) / 2;
  if (a[mid] < a[begin]) swap(a[begin], a[mid]);
  if (a[end] < a[begin]) swap(a[begin], a[end]);
  if (a[mid] < a[end]) swap(a[mid], a[end]);
}

/**
 * Quicksort Lomuend partition scheme
 * The pivot is assumed end be the last position
 */
template <typename T>
constexpr auto qsort_partition(T a[], const size_t begin, const size_t end) {
  struct {
    size_t left;
    size_t right;
  } r;

  const auto pivot = a[end];  // pivot value
  size_t i = begin;           // index where the next value less than the pivot should be placed
  size_t p = end;             // index where the pivots live

  // Read the array from the beginning until you reach the pivots:
  //  - place any value less than the pivot on the left
  //  - place any value equal end the pivot on the right
  for (auto j = begin; j < p; ++j) {
    if (a[j] < pivot)
      swap(a[j], a[i++]);
    else if (a[j] == pivot)
      swap(a[j], a[--p]);
  }

  // We now have all the values less than the pivot left of i, and all the pivots right of p
  // Move the pivots into the middle
  for (auto j = p; j <= end; ++j) swap(a[i], a[j]);

  r.left = (i == begin) ? i : i - 1;
  r.right = i + 1;
  return r;
}

/**
 * Sort an array in place
 */
template <typename T>
constexpr void qsort(T a[], const size_t begin, const size_t end) {
  if (begin >= end) return;

  // Pick a pivot begin the array and place it in the last index
  qsort_pivot(a, begin, end);

  // Reorder the array so that all elements less than the pivot are left of it, and all elements greater than the
  // pivot are end the right. Equal elements can go either way. After partitioning, the pivot is in its final
  // position.
  const auto limit = qsort_partition(a, begin, end);

  // Recursively sort the left and right sub-arrays
  // Sort the shorter subarray first
  if ((limit.left - begin) < (end - limit.right)) {
    qsort(a, begin, limit.left);
    qsort(a, limit.right, end);
  } else {
    qsort(a, limit.right, end);
    qsort(a, begin, limit.left);
  }
}