/*
 * SDSPRUNS  S-Function for running-statistics functions.
 *
 *  Updated: D. Orofino, 24-Feb-97
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.12 $  $Date: 2000/05/05 20:16:06 $
 */

#define S_FUNCTION_NAME sdspruns

#include <math.h>

#include "dsp_sim.h"

/* Work around DEC C-compiler bug: */
#ifdef VMS
#include <fp.h>  /* For HUGE_VAL */
#endif

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

#define ITER_IDX  0
#define NUM_IWORK 1

typedef enum {
    fcnMin,
    fcnMax,
    fcnMean,
    fcnVar,
    fcnStdDev,
    fcnRMS
} FcnType;


#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S)
{
    /*
     * One parameter, a scalar double (0 or 1, only):
     */
    if ( !mxIsDouble(FCN_TYPE)    ||
         (mxGetM(FCN_TYPE) != 1)  ||
         (mxGetN(FCN_TYPE) != 1)  ||
         (mxGetPr(FCN_TYPE)[0]<0) ||
         (mxGetPr(FCN_TYPE)[0]>5)  ) {
        ssSetErrorStatus(S, "Function type must be a scalar in the range [0,5].");
    }
}
#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, DYNAMICALLY_SIZED);
    ssSetDirectFeedThrough(S, 1);
    ssSetNumSampleTimes(   S, 1);
    ssSetNumRWork(         S, DYNAMICALLY_SIZED);
    ssSetNumIWork(         S, NUM_IWORK);
    ssSetOptions(          S, SS_OPTION_EXCEPTION_FREE_CODE |
                              SS_OPTION_USING_ssGetUPtrs);
}


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


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    ssSetIWorkValue(S, ITER_IDX, 0);  /* Reset iteration counter */

#ifdef MATLAB_MEX_FILE
    if (ssGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
        ssSetErrorStatus(S,"Block must have a discrete sample time > 0");
        return;
    }
#endif
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
        UPtrsType uptr    = ssGetUPtrs(S);
        FcnType   ftype   = (FcnType)((int_T)mxGetPr(FCN_TYPE)[0]);
        int_T     width   = ssGetNumInputs(S);
        int_T    *p_iters = ssGetIWork(S) + ITER_IDX;
        int_T     N       = *p_iters;

        /*
         * The last input element is a reset line.
         * If non-zero, all states are to be reset.
         */
        if (*uptr[--width] != 0.0) {
            ssSetIWorkValue(S, ITER_IDX, 0);
			N = 0;  /* Get the current iteration count - again! */
        }

        /*
         * Update running statistics:
         */
        switch (ftype) {

        case fcnMin:
            {
                real_T *min = ssGetRWork(S);

                if (N == 0) {
                    while (width-- > 0) {
                        *min = **(uptr++);  /* RWork is running min values */
                        *y++  = *min++;
	            }
                    *p_iters = 1;  /* Set N=1 */

                } else {
                    while (width-- > 0) {
                        real_T val = **(uptr++);
                        if (val < *min) {
                            *min = val;
                        }
                        *y++ = *min++;
	            }
                }
            }
	    break;


        case fcnMax:
            {
                real_T *max = ssGetRWork(S);

                if (N == 0) {
                    while (width-- > 0) {
                        *max = **(uptr++);
                        *y++ = *max++;
                    }
                    *p_iters = 1;  /* Set N=1 */

                } else {
                    while (width-- > 0) {
                        real_T val = **(uptr++);
                        if (val > *max) {
                            *max = val;
                        }
                        *y++ = *max++;
                    }
                }
            }
            break;


        case fcnMean:
            {
                real_T *curMean = ssGetRWork(S);

                if (N == 0) {
                    while(width-- > 0) {
                        *curMean = **(uptr++);
                        *y++ = *curMean++;
                    }
                } else {
                    while(width-- > 0) {
                        *curMean = (*curMean * N + **(uptr++)) / (N+1);
                        *y++ = *curMean++;
                    }
                }
                ++(*p_iters);

#ifdef MATLAB_MEX_FILE
                if (*p_iters == 0) {
                    mexWarnMsgTxt("Internal counter overflow detected in Running Mean block.\n"
                                  "To avoid counter overflow, periodically reset the block,\n"
                                  "or decrease the amount of time that the simulation runs.\n");
                }
#endif
        }
        break;

        
        case fcnVar:
        case fcnStdDev:
            {
       	        /*
	         * now compute the running variance.  
	         * It is computed using the following Pseudo code algorithm:
	         *    for each vector element
	         *      if reset, 
	         *           s = 0;  % sum of squares
	         *           c = 0;  % current sum of samples
	         *           N = 0;  % number of samples
	         *      end
	         *      s = s + u^2;
	         *      c = c + u;
	         *      N = N+1;
	         *      if N==1
	         *          var = 0;
	         *      else
	         *          var = (s - c^2/N)/(N-1);
                 *      end
                 *    end  (for loop)
	         */
                real_T *sum   = ssGetRWork(S);
                real_T *sqsum = ssGetRWork(S) + width;

                if (N == 0) {
                    while(width-- > 0) {
                        real_T val = **(uptr++);
                        *sum++    = val;
                        *sqsum++  = val * val;
                        *y++      = 0.0;
                    }

                } else {
                    while(width-- > 0) {
                        real_T val = **(uptr++);
                        *sum   += val;
                        *sqsum += val * val;
                        {
                            real_T s = *sqsum++;
                            real_T c = *sum++;
                            real_T t = (s - c*c/(N+1)) / N;
                            *y++ = (ftype == fcnVar) ? t : sqrt(t);
                        }
                    }
                }
                ++(*p_iters);

#ifdef MATLAB_MEX_FILE
                if (*p_iters == 0) {
                    mexWarnMsgTxt("Internal counter overflow detected in Running Variance block.\n"
                                  "To avoid counter overflow, periodically reset the block,\n"
                                  "or decrease the amount of time that the simulation runs.\n");
                }               
#endif
            }
	    break;


        case fcnRMS:
            {
    	        real_T *msqsum = ssGetRWork(S); /* mean squared sum */

                if (N == 0) {
                    while(width-- > 0) {
                        real_T val = **(uptr++);
                        *msqsum    = val * val;
                        *y++       = sqrt(*msqsum++);  /* We want abs value here */
                    }
                } else {
                    while(width-- > 0) {
                        real_T val = **(uptr++);
                        *msqsum     = (*msqsum * N + val * val) / (N+1);
                        *y++       = sqrt(*msqsum++);
                    }
                }
                ++(*p_iters);

#ifdef MATLAB_MEX_FILE
                if (*p_iters == 0) {
                        /* Iteration counter exceeded maximum: */
                        mexWarnMsgTxt("Internal counter overflow detected in Running RMS block.\n"
                                      "To avoid counter overflow, periodically reset the block,\n"
                                      "or decrease the amount of time that the simulation runs.\n");
                }
#endif
            }
            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)
{
}


#ifdef MATLAB_MEX_FILE
#define MDL_GET_INPUT_PORT_WIDTH
/* Function: mdlGetInputPortWidth =============================================
 *
 */
static int_T mdlGetInputPortWidth(SimStruct *S, int_T outputPortWidth)
{
    return(outputPortWidth+1);
}
#endif


#ifdef MATLAB_MEX_FILE
#define MDL_GET_OUTPUT_PORT_WIDTH
/* Function: mdlGetOutputPortWidth ============================================
 *
 */
static int_T mdlGetOutputPortWidth(SimStruct *S, int_T inputPortWidth)
{
    return(inputPortWidth-1);
}
#endif


/* Function: mdlSetWorkWidths =================================================
 * Abstract:
 *
 */
#ifdef MATLAB_MEX_FILE
#define MDL_SET_WORK_WIDTHS
static void mdlSetWorkWidths(SimStruct *S)
{
    FcnType ftype = (FcnType)((int_T)mxGetPr(FCN_TYPE)[0]);
    int_T   N     = (ssGetNumInputs(S)-1);

    /* Need twice as much storage for var and std: */
    if ((ftype == fcnVar) || (ftype == fcnStdDev)) {
        N *= 2;
    }

    ssSetNumRWork(S, N);
}
#endif


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