/* Implement Quicksort algorithm using indices (qid)
 * Note: this algorithm is different from MATLAB's sorting
 * for complex values with same magnitude.
 *
 * Sorts an array of doubles based on the "Quicksort" algorithm,
 * using an index vector rather than the data itself.
 *
 *  Copyright (c) 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.3 $  $Date: 2000/03/03 21:28:13 $
 */

#include "dspqsrt_rt.h"

#define qid_Swap(i,j) {int_T t        = *(qid_index+i); \
                       *(qid_index+i) = *(qid_index+j); \
                       *(qid_index+j) = t;}

#define qid_Order(i,j) {if( *(qid_array+*(qid_index+i)) > \
                            *(qid_array+*(qid_index+j)) ) qid_Swap(i,j)}

#define qid_Order3(i,j,k) {qid_Order(i,j); qid_Order(i,k); qid_Order(j,k);}


static int_T qid_Partition(const real_T *qid_array, int_T *qid_index,
                           int_T i, int_T j, int_T pivot )
{
    real_T pval = *(qid_array + *(qid_index + pivot));

    while (i <= j) {
        while( *( qid_array + *(qid_index+i) ) <  pval) {
            ++i;
        }
        while( *( qid_array + *(qid_index+j) ) >= pval) {
            --j;
        }
        if (i<j) {
            qid_Swap(i,j)
            ++i; --j;
        }
    }
    return(i);
}


static boolean_T qid_FindPivot(const real_T *qid_array, int_T *qid_index,
                               int_T i, int_T j, int_T *pivot )
{
    int_T  mid = (i+j)/2;
    int_T  k;
    real_T a, b, c;

    qid_Order3(i,mid,j);    /* order the 3 values */
    a = *(qid_array + *(qid_index + i  ));
    b = *(qid_array + *(qid_index + mid));
    c = *(qid_array + *(qid_index + j  ));

    if (a < b) {   /* pivot will be higher of 2 values */
        *pivot = mid;
        return((boolean_T)1);
    }
    if (b < c) {
        *pivot = j;
        return((boolean_T)1);
    }
    for (k=i+1; k <= j; k++) {
        real_T d = *(qid_array + *(qid_index + k));
        if (d != a) {
          *pivot = (d < a) ? i : k ;
          return((boolean_T)1);
        }
    }
    return((boolean_T)0);
}


/* The recursive quicksort routine: */
void qid_RecSort(const real_T *qid_array, int_T *qid_index,
                        int_T i, int_T j )
{
    int_T pivot;
    if (qid_FindPivot(qid_array, qid_index, i, j, &pivot)) {
        int_T k = qid_Partition(qid_array, qid_index, i, j, pivot);
        qid_RecSort(qid_array, qid_index, i, k-1);
        qid_RecSort(qid_array, qid_index, k, j);
    }
}

/* [EOF] dspqsrt_rt.c */