#include <octave/oct.h>
#include <octave/lo-mappers.h>
#include <octave/config.h>
#include <octave/quit.h>

static Array<int>
create_index_array (int n)
{
  Array<int> l (n+2);

  l (0) = 1;

  for (int i = 1; i < n - 1; i++)
    l (i) = -(i+2);

  l (n-1) = 0;
  l (n) = 0;
  l (n+1) = 2;

  return l;
}

#define SORT_INIT_PHASE(n) \
  int s = 0; \
  int t = n + 1; \
  int p = l (s); \
  int q = l (t); \
  if (q == 0) \
     break

#define SORT_COMMON_CODE \
  p = -p; \
  q = -q; \
  if (q == 0) \
    { \
      l (s) = (l (s) < 0) \
	? ((p < 0) ? p : -p) \
	  : ((p >= 0) ? p : -p); \
      l (t) = 0; \
      break; \
    } \

#define SORT_REORDER_PHASE_ONE \
  l (s) = (l (s) < 0) \
    ? ((q < 0) ? q : -q) \
      : ((q >= 0) ? q : -q); \
  s = q; \
  q = l (q); \
  if (q <= 0) \
    { \
      l (s) = p; \
      s = t; \
      do \
	{ \
	  t = p; \
	  p = l (p); \
	} \
      while (p > 0); \
      SORT_COMMON_CODE; \
    } \

#define SORT_REORDER_PHASE_TWO \
  l (s) = (l (s) < 0) \
    ? ((p < 0) ? p : -p) \
      : ((p >= 0) ? p : -p); \
  s = p; \
  p = l (p); \
  if (p <= 0) \
    { \
      l (s) = q; \
      s = t; \
      do \
	{ \
	  t = q; \
	  q = l (q); \
	} \
      while (q > 0); \
      SORT_COMMON_CODE; \
    }

#define DO_SORT(n, condition) \
  while (1) \
    { \
      SORT_INIT_PHASE(n); \
      while (1) \
	{ \
          OCTAVE_QUIT; \
	  if (condition) \
	    { \
	      SORT_REORDER_PHASE_ONE; \
	    } \
	  else \
	    { \
	      SORT_REORDER_PHASE_TWO; \
	    } \
	} \
    }

#define DO_SORT_IEEE754(n) \
  while (1) \
    { \
      SORT_INIT_PHASE(n); \
      while (1) \
	{ \
          OCTAVE_QUIT; \
	  unsigned long long v1 = vec_int[p-1]; \
	  unsigned long long v2 = vec_int[q-1]; \
	  bool s1 = (mask & v1) != 0; \
	  bool s2 = (mask & v2) != 0; \
	  if ((!s1 & s2) || ( s1 & s2 & (v1 < v2)) || \
		( !s1 & !s2 & (v1 > v2))) \
	    { \
	      SORT_REORDER_PHASE_ONE; \
	    } \
	  else \
	    { \
	      SORT_REORDER_PHASE_TWO; \
	    } \
	} \
    }

#define VECTOR_CREATE_RETURN_VALUES(vs, v) \
  int k = l (0); \
  idx (0) = k; \
  vs (0) = v (k-1); \
  for (int i = 1; i < n; i++) \
    { \
      OCTAVE_QUIT; \
      k = l (static_cast<int> (idx (i-1))); \
      idx (i) = k; \
      vs (i) = v (k-1); \
    }

DEFUN_DLD (mysort, args, , "") {
  octave_value_list retval;

  RowVector v = args(0).row_vector_value();
  int n = v.capacity();
  RowVector vs (n);
  RowVector idx (n);
  
  // Simple bubble sort for now
  if (n == 1)
    {
      retval(0) = v;
      retval(1) = RowVector (n, 1.0);
      return retval;
    }
  else if (args.length() > 1)
    {
      // Use integer compare al la IEEE754
      double * vec_dbl = v.fortran_vec();
      unsigned long long *vec_int = (unsigned long long *)vec_dbl;
      unsigned long long mask = (unsigned long long)1 << ((sizeof(unsigned long long)<<3) -1);

      Array<int> l = create_index_array (n);

      DO_SORT_IEEE754 (n);

      VECTOR_CREATE_RETURN_VALUES (vs, v);

      retval(1) = idx;
      retval(0) = vs;
    }
  else
    {
      Array<int> l = create_index_array (n);

      DO_SORT (n, (xisnan (v (p-1)) || v (p-1) > v (q-1)));

      VECTOR_CREATE_RETURN_VALUES (vs, v);

      retval(1) = idx;
      retval(0) = vs;
    }

  return retval;
}