#include <bassert.h>
#include <mem_syms.h>

void merge(int *lout, int *list1, int len1, int *list2, int len2) {
/*

Merge from 2 input lists into a user supplied output list.

Pre: list list1 of length len1, list list2 of length len2 
     both sorted in ascending order.

     list lout is at least length len1+len2

Post: merges list1 and list2 by increasing order into lout, which will
    have len1+len2 elements

Invariant: list1 and list2 are unchanged.

This version solves the memory leak problem.  It also uses pointer 
arithmetic which is faster than array indexing.
*/

  while ( len1 > 0 || len2 > 0 ) {
      
    // pick from list 1 if list 2 is empty, or list 1 < list 2
    if ( (len2 == 0) || (len1 > 0 && *list1 <= *list2) ) {
      // note use of post increment, which performs the assignment, 
      // *lout = *list1, // then increments the pointers
      *lout++ = *list1++;
      len1--; 
    } else {
      *lout++ = *list2++;
      len2--; 
    }
  }
}

/*
    pre: listin is a pointer to a list of len elements
        needs sufficient stack space to allocate a temporary list, plus
        work variables.

    post: the contents of listin will be rearranged to be sorted in 
        ascending order.

    The merge sort works by breaking the input into blocks of length
    block_len, each of which is ordered.  This is the source of the 
    merger.  

    Then adjacent pairs of blocks are merged into a block of size 
    2*block_len that is placed in the destination.

    The source and destination are swapped and the process continues 
    util the block_len is bigger than the list to be sorted.
*/

void mergesort(int *listin, int len) {
  int block_len; // current length of merge block

  // listin and tmp alternate roles as source and destination
  // 0 => destination is tmp, 1 => destination is listin
  byte buf_parity; 

  // a pair of adjacent blocks to be merged has a left and right block

  int *src;     // current position of left array in source
  int *dst;     // current position in destination

  int remain;   // elements remaining to be processed   
  int left_len; // length of left block to merge
  int right_len;  // length of right block to merge
  int merged_len; // length of destination merge

  Serial.print("Avail mem: "); Serial.println(AVAIL_MEM);
  bassert( AVAIL_MEM > len * sizeof(*listin) + 200, 7);

  int tmp[len];

  if ( len <= 1 ) { return; }

  /* go through the list working on blocks of length block_len = 2 ** k

    src - pointer to source data, in blocks of length block_len, except
        possibly for the last block which could be partial.

    dst - pointer to destination where merged blocks are placed

    buf_parity
        true => merging from listin into tmp
        false => merging from tmp into listin
  */

  // keep sorting so long as block size is less than list length
  // if = or greater, list is sorted
  buf_parity = 1;
  block_len = 1;

  while ( block_len < len ) {

    // merge from src into dst
    if ( buf_parity ) {
      src = listin;
      dst = tmp;
      }
    else {
      src = tmp;
      dst = listin;
      }

    // number of elements remaining to be merged
    remain = len;

    while ( remain > 0 ) {

      // merge the next two blocks together
      if ( remain <= block_len ) {
        // 1 block or less remaining
        left_len = remain;
        right_len = 0;
        }
      else if ( remain < block_len + block_len ) {
        // 1 full block, and a partial last block
        left_len = block_len;
        right_len = remain - block_len;
        }
      else {
        // 2 full blocks to merge
        left_len = block_len;
        right_len = block_len;
        }

      // left list starts at current src, right list at src+left_len, 
      // right list ends at src+left_len+right_len -1 

      merged_len = left_len + right_len;
    
      merge(dst, src, left_len, src+left_len, right_len);

      // move along both src and desitination to next pair of blocks
      dst += merged_len;
      src += merged_len;
      remain = remain - merged_len;
      }

    // swap src and dst
    buf_parity = ! buf_parity;
    block_len = block_len + block_len;
    }

  // if sorted list is in tmp, copy back into input list
  if ( ! buf_parity ) {
    int tlen = len;
    src = tmp;
    dst = listin;
    while ( tlen-- ) { *dst++ = *src++; }
    }
  return;
}

unsigned long start_time;
unsigned long stop_time;

void start_clock() {
    start_time = micros();
    }

unsigned long int stop_clock() {
    unsigned long elapsed_time;
    stop_time = micros();
    elapsed_time = stop_time - start_time;
    Serial.print("Elapsed time ");
    Serial.print(elapsed_time);
    Serial.println(" uS");
    return elapsed_time;
    }

byte is_sorted(int *l, int len) {
    for (int i=0; i < len-1; i++) {
        if ( l[i] > l[i+1] ) { return 0; }
        }
    return 1;
    }

void display_list(int *l, int len) {
  for (int i=0; i < len; i++ ) { 
    Serial.print(l[i]); Serial.print(" "); 
  }
}

void setup() {
  Serial.begin(9600);
  
  for (int len = 100; len < 10000; len += 200) {
      Serial.print("Testing with len "); Serial.println(len);

      // make sure we have enough memory
      bassert(AVAIL_MEM > len * sizeof(int) + 256, 3);

      int l[len];
      
      for (int i=0; i < len; i++ ) { l[i] = random(100); }
      
      Serial.println("In:");
      // display_list(l, len);
      Serial.println("");

      Serial.println("Out:");
      
      start_clock();
      mergesort(l, len);
      stop_clock();
      if ( is_sorted(l, len) ) {
        Serial.println("Success: list is sorted");
        }
      else {
        Serial.println("Error: list not sorted!");
        display_list(l, len);
        }

      // display_list(l, len);
  }
}

void loop() {
}