/*
 * SDSPSORT DSP Blockset S-Function for sorting real vectors.
 *
 * ORDER_ARG: 1=ascending, 2=descending, 3=median value
 *
 *  Updated: D. Orofino, 25-Jun-97
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.10 $  $Date: 2000/05/05 20:16:07 $
 */

#define S_FUNCTION_NAME sdspsort

#include "dsp_sim.h"

#define ORDER_ARG   ssGetArg(S,0)
#define NUM_ARGS    1

typedef enum {
    fcnMedian  = 0,
    fcnAscend  = 1,
    fcnDescend = 2
} FcnType;


/* Implement Quicksort algorithm using indices (qid)
 *
 * Sorts an array of doubles based on the "Quicksort" algorithm,
 * using an index vector rather than the data itself.
 */
#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: */
static 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);
    }
}


#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
    /*
     * One parameter, a scalar double (0=ascending, 1=descending, only):
     */
    if ( !mxIsDouble(ORDER_ARG)         ||
         (mxGetM(ORDER_ARG) != 1)       ||
         (mxGetN(ORDER_ARG) != 1)       ||
         ( (mxGetPr(ORDER_ARG)[0] != 0.0) &&
           (mxGetPr(ORDER_ARG)[0] != 1.0) &&
           (mxGetPr(ORDER_ARG)[0] != 2.0)  ) ) {
        ssSetErrorStatus(S, "Argument must be 1 (ascending), 2 (descending),"
                            " or 0 (median value) only.");
    }
}
#endif


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NUM_ARGS);

#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;
    mdlCheckParameters(S);
    if (ssGetErrorStatus(S) != NULL) return;
#endif

    ssSetNumInputs(        S, DYNAMICALLY_SIZED);
    ssSetNumOutputs(       S, (mxGetPr(ORDER_ARG)[0] == 0) ? 1 : DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetNumIWork(         S, DYNAMICALLY_SIZED);
    ssSetOptions(          S, SS_OPTION_EXCEPTION_FREE_CODE);
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTimeEvent(S, 0, 0.0);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    /* 
     * Initialize sort indices to ascending integers in range [0,N-1].
     * After that, the integers are in a "random" order, which
     * is efficient for sorting purposes on subsequent calls.
     */
    int_T *index = ssGetIWork(S);
    int_T  N     = ssGetNumInputs(S);
    int_T  i;
    for (i=0; i < N; i++) {
        *index++ = i;
    }
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    int_T    N     = ssGetNumInputs(S);
    int_T   *index = ssGetIWork(S);
    FcnType  ftype = (FcnType)((int_T)mxGetPr(ORDER_ARG)[0]);
    int_T    i;

    /* Sort input vector: */
    qid_RecSort(u, index, 0, N-1);

    switch(ftype) {

    case fcnMedian:
        /* Median value of elements: */
        if (N % 2 == 0) {
            /* Even number of elements - interpolate: */
            *y = 0.5 * (u[index[N/2-1]] + u[index[N/2]]);
        } else {
            *y = u[index[(N-1)/2]];
        }
        break;

    case fcnAscend:
        /* Ascending: */
        for(i=0; i<N; i++) {
            *y++ = u[index[i]];
        }
        break;

    case fcnDescend:
        /* Descending: */
        for(i=N-1; i>=0; i--) {
            *y++ = u[index[i]];
        }
        break;

    }
}

static void mdlUpdate(real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
}


static void mdlDerivatives(real_T *dx, const real_T *x, const real_T *u, 
                           SimStruct *S, int_T tid)
{
}


static void mdlTerminate(SimStruct *S)
{
}


#include "dsp_trailer.c"
 
/* [EOF]: sdspsort.c */