#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <assert.h>

#define LEN(x) (sizeof(x)/sizeof((x)[0]))

void insertion_sort(int *numbers, int len) {
  for (int sorted_size = 0; sorted_size < len; sorted_size++) {
    int next_item = numbers[sorted_size];

    /* Find place to insert next item */
    int pos;
    for (pos = 0; pos < sorted_size; pos++)
      if (numbers[pos] > next_item) break;

    /* Move over other items to make room */
    for (int ii = sorted_size; ii > pos; ii--) numbers[ii] = numbers[ii-1];

    numbers[pos] = next_item;
  }
}

void selection_sort(int *numbers, int len) {
  for (int sorted_size = 0; sorted_size < len; sorted_size++) {
    /* Find smallest item */
    int smallest = sorted_size;
    for (int ii = sorted_size; ii < len; ii++)
      if (numbers[ii] < numbers[smallest]) smallest = ii;

    /* Swap it in */
    int tmp = numbers[sorted_size];
    numbers[sorted_size] = numbers[smallest];
    numbers[smallest] = tmp;    
  }
}

/* Simple binary heap routines implementing a max-heap. */

void heap_push(int *numbers, int *len, int num) {
  int pos = (*len)++;
  /* Bubble number up from bottom */
  while (pos > 0 && num >= numbers[(pos-1)/2]) {
    numbers[pos] = numbers[(pos-1)/2];
    pos = (pos-1)/2;
  }
  numbers[pos] = num;
}

int heap_pop(int *numbers, int *len) {
  int rv = numbers[0];  /* Remove the number at the top of the heap; */
  int num = numbers[--*len]; /* replace it with the one from end of heap. */

  /* Trickle it down to its correct position. */
  int pos = 0, kidpos = 1;
  while (kidpos < *len) {
    if (kidpos + 1 < *len && numbers[kidpos+1] > numbers[kidpos])
      kidpos++;
    if (num >= numbers[kidpos]) break;

    numbers[pos] = numbers[kidpos];
    pos = kidpos;
    kidpos = 2*pos + 1;         /* leftmost child */
  }

  numbers[pos] = num;

  return rv;
}

void assert_heap_valid(int *numbers, int *len) {
  for (int ii = 0; ii*2 + 1 < *len; ii++) {
    assert(numbers[ii] >= numbers[ii*2 + 1]);
    if (ii*2 + 2 < *len)
      assert(numbers[ii] >= numbers[ii*2 + 2]);
  }
}

void slow_heapsort(int *numbers, int len) {
  int lenb = 0;
  for (int ii = 0; ii < len; ii++) heap_push(numbers, &lenb, numbers[ii]);
  for (int ii = len; ii > 0; ii--) numbers[ii-1] = heap_pop(numbers, &lenb);
}

int sorted(int *numbers, int len) {
  for (int ii = 0; ii < len-1; ii++) if (numbers[ii] > numbers[ii+1]) return 0;
  return 1;
}

int set_equal(int *sorted, int *unsorted, int len) {
  for (int ii = 0; ii < len; ii++) {
    int count = 0;
    for (int jj = ii; jj < len; jj++) {
      if (sorted[jj] != sorted[ii]) break;
      count++;
    }

    int unsorted_count = 0;
    for (int jj = 0; jj < len; jj++)
      if (unsorted[jj] == sorted[ii]) unsorted_count++;

    if (count != unsorted_count) return 0;

    ii += count-1; /* So we only see the beginning of each sorted run */
  }
  return 1;
}

int numbers[] = { 3, 51, 0, 3, 11, 9, 100, 99, 98, 97, 15 };
int buf[100];

void test(void (*sort)(int *, int)) {
  memcpy(buf, numbers, sizeof(numbers));
  sort(buf, LEN(numbers));
  if (!sorted(buf, LEN(numbers))) abort();
  if (!set_equal(buf, numbers, LEN(numbers))) abort();
}

int main() {
  test(insertion_sort);
  test(selection_sort);
  test(slow_heapsort);
  return 0;
}