/*
 * SSTATFCN  S-Function for statistics functions.
 * DSP Blockset S-Function for vector and running statistics functions.
 *      This SIMULINK S-function computes statistical functions of the input 
 *	vector;	it is called by Statistics blocks in the DSP Blockset.
 *
 *  Updated: D. Orofino, 24-Feb-97
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.14 $  $Date: 2000/05/05 20:16:16 $
 */

#define S_FUNCTION_NAME sstatfcn

#include <string.h>   /* strcmp() */
#include <math.h>

#include "dsp_sim.h"

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

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

typedef enum {
    fcnMin,
    fcnMax,
    fcnRunningMin,
    fcnRunningMax,
    fcnRunningMean,
    fcnRunningVariance,
    fcnRunningRMS
} FcnType;


#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
    const char *msg = NULL;

    if (!mxIsChar(FCN_NAME)) {
        msg = "Function name must be a string.";
        goto ERROR_EXIT;
    }

ERROR_EXIT:
    if (msg != NULL) {
        ssSetErrorStatus(S,msg);
    }
}
#endif


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(     S, NUM_ARGS);      /* number of input arguments */

#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Simulink will report a parameter mismatch error */
    }
#endif

    ssSetNumContStates(    S, 0);                 /* number of continuous states */
    ssSetNumDiscStates(    S, 0);                 /* number of discrete states */
    ssSetNumInputs(        S, DYNAMICALLY_SIZED); /* number of inputs */
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED); /* number of outputs */
    ssSetDirectFeedThrough(S, 1);                 /* direct feedthrough flag */
    ssSetNumSampleTimes(   S, 1);                 /* number of sample times */
    ssSetNumRWork(         S, DYNAMICALLY_SIZED); /* number of real work vector elements */
    ssSetNumIWork(         S, 2);                 /* number of integer work vector elements */
    ssSetNumPWork(         S, 0);                 /* number of pointer work vector elements */
    ssSetNumModes(         S, 0);
    ssSetNumNonsampledZCs( S, 0);
    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)
{
    real_T *RWork   = ssGetRWork(S);
    int    *IWork   = ssGetIWork(S);
    int    numRWork = ssGetNumRWork(S);
    char   fcnName[16];
    int    i;
    /*
     * get the function name from the input argument, bail out
     * if for some reason mxGetString can't convert it.
     */
#ifdef MATLAB_MEX_FILE
    if (mxGetString(FCN_NAME, fcnName, sizeof(fcnName)) != 0) {
	ssSetErrorStatus(S,"Error in function name specification");
        return;
    }
#else    
    /* in case of code generation, don't bother with error checking: */
    /* (works around bug in mxGetString as defined in cg_matrx.h)    */
    mxGetString(FCN_NAME, fcnName, sizeof(fcnName));
#endif

    /*
     * translate name to enum value
     */
    if (strcmp(fcnName,"min") == 0) {
	IWork[0] = (int)fcnMin;

    } else if (strcmp(fcnName,"max") == 0) {
	IWork[0] = (int)fcnMax;

    } else if (strcmp(fcnName,"runningmin") == 0) {
	IWork[0] = (int)fcnRunningMin;
	for (i = 0; i < numRWork; i++) {
	  RWork[i] = 1.0e38;
        }
    
    } else if (strcmp(fcnName,"runningmax") == 0) {
	IWork[0] = (int)fcnRunningMax;
	for (i = 0; i < numRWork; i++) {
	  RWork[i] = -1.0e38;
        }

    } else if (strcmp(fcnName,"runningmean") == 0) {
	IWork[0] = (int)fcnRunningMean;
	IWork[1] = 0;
	for (i = 0; i < numRWork; i++) {
	  RWork[i] = (real_T)0.0;
        }

    } else if (strcmp(fcnName,"runningvar") == 0) {
	IWork[0] = (int)fcnRunningVariance;
	IWork[1] = 0;
	for (i = 0; i < numRWork; i++) {
	  RWork[i] = (real_T)0.0;
        }

    } else if (strcmp(fcnName,"runningrms") == 0) {
	IWork[0] = (int)fcnRunningRMS;
	IWork[1] = 0;
	for (i = 0; i < numRWork; i++) {
	  RWork[i] = (real_T)0.0;
        }
    }
#ifdef MATLAB_MEX_FILE
    else {
        ssSetErrorStatus(S,"Error in function name specification");
        return;
    }
#endif
}

static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    FcnType ftype    = (FcnType) ssGetIWorkValue(S, 0);
    int     *IWork   = ssGetIWork(S);
    int     numRWork = ssGetNumRWork(S);
    int     width    = ssGetNumInputs(S);
    int     i;
    
    /* NOTE REGARDING INPUT WIDTH FOR MIN and MAX
       for max and min, the input width is two greater than the
       actual width, so that there will always be at least 3 outputs
       as needed for input to the demux at the output of this function.
       The last two input elements are ignored by the code.
       This is so we can use this same MEX-file for the running blocks
       (which output a vector the same size as the input) and the
       min/max blocks (which output two scalars).
     */

    switch (ftype) {

    case fcnMin:
        /*
	 * fcnMin - find the minimum value of the input vector
	 */
        {
            real_T val = *u++;     /* min is 1st element   */
            int    idx = 0;        /* index of 1st element */
            for (i=1; i<(width-2); i++) {
                if (*u < val) {
                    val = *u; idx = i;
                }
                u++;
            }
            y[0] = val;
            y[1] = (real_T)(idx+1); /* Convert 0-based C-index to 1-based MATLAB index */
        }
        break;

    
    case fcnMax:
        /*
         * fcnMax - find the maximum value of the input vector
         */
        {
            real_T val = *u++;     /* max is 1st element   */
            int    idx = 0;        /* index of 1st element */
            for (i=1; i<(width-2); i++) {
                if (*u > val) {
                    val = *u; idx = i;
                }
                u++;
            }
            y[0] = val;
            y[1] = (real_T)(idx+1); /* Convert 0-based C-index to 1-based MATLAB index */
        }
        break;


    case fcnRunningMin:
        /*
         * fcnRunningMin - running minimum, find the minimum of the input vector
         *                 and the current output
         */
        if (ssIsMajorTimeStep(S)) {
            real_T *runningMin = ssGetRWork(S);

	    /*
	     * The running min has a reset input which is the last value
	     * in the input vector.  If this input is nonzero, reset the
	     * running min to very big.
	     */
	    if (u[--width] != 0.0) {
	      for (i = 0; i < numRWork; i++) {
		runningMin[i] = HUGE_VAL;
              }
	    }
            /* Compute new running min: */
            while (width-- > 0) {
                if (*u < *runningMin) {
                    *runningMin = *u;
                }
                u++;
                *y++ = *runningMin++;
	    }
        }
	break;


    case fcnRunningMax:
        /*
         * fcnRunningMax - running maximum, find the maximum of the input vector
         *                 and the current output
         */
        if (ssIsMajorTimeStep(S)) {
	    real_T *runningMax = ssGetRWork(S); 

	    /*
	     * The running max has a reset input which is the last value
	     * in the input vector.  If this input is nonzero, reset the
	     * running max for that input to very negative big.
	     */
	    if (u[--width] != 0.0) {
                for (i = 0; i < numRWork; i++) {
		    runningMax[i] = -HUGE_VAL;
                }
            }
            /* Compute new running max: */
            while (width-- > 0) {
                if (*u > *runningMax) {
                    *runningMax = *u;
                }
                u++;
                *y++ = *runningMax++;
	    }
	}
	break;


    case fcnRunningMean:
        /*
         * fcnRunningMean - running mean, compute the mean of the current and 
         *                  all prior inputs
         */

	if (ssIsMajorTimeStep(S)) {
	    int    *numIters = ssGetIWork(S) + 1;
	    real_T *curMean  = ssGetRWork(S);

            /*
             * The running mean has a reset input which is the last value
             * in the input vector.  If this input is nonzero, reset the
             * running mean to zero.
             */
            if (u[--width] != 0.0) {
                *numIters = 0;
                /* Reset memory to zero: */
                for (i=numRWork; i-- != 0; ) {
                    *curMean++ = (real_T)0.0;
                }
                curMean = ssGetRWork(S);  /* Reset pointer */
            }
	  
            /*
             * the running mean is the sum of this frame's mean and
             * all prior frame means divided by the number of times
             * that the running mean has been computed
             */
            while(width-- > 0) {
	        *curMean = (*curMean * *numIters + *u++) / (*numIters + 1);
	        *y++ = *curMean++;
            }

            /* Increment number of iterations: */
            (*numIters)++;
#ifdef MATLAB_MEX_FILE
            if (*numIters == 0) {
                /* Iteration counter exceeded maximum: */
                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 fcnRunningVariance:
        /*
	 * if this is a major time step, compute the running variance.
	 * We real_T the numRWork here to accomodate storage for both
	 * the current mean and the sum of squares for each input.
	 */
        if (ssIsMajorTimeStep(S)) {
	    int    *numIters     = ssGetIWork(S) + 1;
	    int    numRWork      = ssGetNumRWork(S)-1;
            int    width         = (ssGetNumInputs(S)-1)/2;
            real_T *curMean      = ssGetRWork(S);
            real_T *sumOfSquares = ssGetRWork(S) + width;

            /*
             * The running variance has a reset input which is the last value
             * in the input vector.  If this input is nonzero, reset the
             * running variance to zero.
             */
            if (u[ssGetNumInputs(S)-1] != 0.0) {
                *numIters  = 0;
                /* Reset memory to zero: */
                for (i=numRWork; i-- != 0; ) {
                    *curMean++ = (real_T)0.0;
                }
                curMean = ssGetRWork(S);  /* Reset pointer */
            }

       	    /*
	     * 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)
	     */
            while(width-- > 0) {
                *curMean      += *u;
                *sumOfSquares += *u * *u;
                u++;
                {
                    int    N = *numIters;
                    real_T s = *sumOfSquares++;
                    real_T c = *curMean++;
                    *y++ = (N==0) ? 0.0 : (s-(c*c/(N+1))) / N;
                }
            }

            /* increment number of iterations: */
	    (*numIters)++;
#ifdef MATLAB_MEX_FILE
            if (*numIters == 0) {
                /* Iteration counter exceeded maximum: */
                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 fcnRunningRMS:
	if (ssIsMajorTimeStep(S)) {
	    real_T *sumOfSquares = ssGetRWork(S);
	    int    *numIters     = ssGetIWork(S) + 1;

            /*
             * The running RMS has a reset input which is the last value
             * in the input vector.  If this input is nonzero, reset the
             * running variance to zero.
             */
            if (u[--width] != 0.0) {
	        *numIters = 0;
                /* Reset memory to zero: */
                for (i=numRWork; i-- != 0; ) {
                    *sumOfSquares++ = (real_T)0.0;
                }
                sumOfSquares = ssGetRWork(S);  /* Reset pointer */
	    }

            /*
             * now compute the running RMS.  It's the square root of the total sum
             * of squares.
             */
            while(width-- > 0) {
                *sumOfSquares = (*sumOfSquares * *numIters + *u * *u) / (*numIters + 1);
                u++;
                *y++ = sqrt( *sumOfSquares++ );
            }

            /* increment number of iterations: */
	    (*numIters)++;
#ifdef MATLAB_MEX_FILE
            if (*numIters == 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;


    default:
        /* Must be an unhandled case: */
#ifdef MATLAB_MEX_FILE
        ssSetErrorStatus(S,"Error in function name specification");
#endif
        break;

    }    /* end of switch */
}


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]: sstatfcn.c */