/*
 * QUANTIZE  Example mex-file for a vectorized quantizer block.
 *           Quantizes the input into steps as specified by the
 *           quantization interval parameter, q.
 *
 *           Syntax:  [sys, x0] = quantize(t, x, u, flag, q);
 * 
 *  Erick Asplund   11/29/93
 *  Copyright (c) 1994 by The MathWorks, Inc.
 *  All Rights Reserved
 */

/*
 * The following #define is used to specify the name of this S-Function.
 */

#define S_FUNCTION_NAME quantize

#include <math.h>

#ifdef MATLAB_MEX_FILE
#include "mex.h"
#endif

/*
 * need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */

#include "simstruc.h"

/*
 * mdlInitializeSizes - initialize the sizes array
 *
 * The sizes array is used by SIMULINK to determine the S-function block's
 * characteristics (number of inputs, outputs, states, etc.).
 */

static void mdlInitializeSizes(S)
    SimStruct * S;
{
    Matrix *quant;
    int size;
    
    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 */
    ssSetNumInputArgs(     S, 1);                  /* number of input arguments */
    ssSetNumRWork(         S, 0);                  /* number of real work vector elements */
    ssSetNumIWork(         S, 0);                  /* number of integer work vector elements */
    ssSetNumPWork(         S, 0);                  /* number of pointer work vector elements */
    
    /*
     * Make sure the quantization interval is specified
     */
    
    if (ssGetNumArgs(S) != 1) {
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt("A quantization interval must be specified.");
#endif
	return;
    }
    
    quant = ssGetArg(S, 0);
    if ((mxGetM(quant) > 1) && (mxGetN(quant) > 1)) {
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt("The quantization interval must be a vector.");
#endif
    }
    
    size = mxGetM(quant) * mxGetN(quant);
    if (size == 0) {
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt("The quantization interval cannot be an empty matrix.");
#endif
    }
    
    /* 
     * If the quantization interval is a vector, set the number 
     * of inputs and outputs to be the same size as it.
     */
    
    if (size > 1) {
	ssSetNumInputs( S, size);
	ssSetNumOutputs(S, size);
    }
}

/*
 * mdlInitializeSampleTimes - initialize the sample times array
 *
 * This function is used to specify the sample time(s) for your S-function.
 * If your S-function is continuous, you must specify a sample time of 0.0.
 * Sample times must be registered in ascending order.  If your S-function
 * is to acquire the sample time of the block that is driving it, you must
 * specify the sample time to be INHERITED_SAMPLE_TIME.
 */

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

/*
 * mdlInitializeConditions - initialize the states
 *
 * In this function, you should initialize the continuous and discrete
 * states for your S-function block.  The initial states are placed
 * in the x0 variable.  You can also perform any other initialization
 * activities that your S-function may require.
 */

static void mdlInitializeConditions(x0, S)
    double * x0;
    SimStruct * S;
{
}

/*
 * mdlOutputs - compute the outputs
 *
 * In this function, you compute the outputs of your S-function
 * block.  The outputs are placed in the y variable.
 */

static void mdlOutputs(y, x, u, S, tid)
    double * y;
    double * x;
    double * u;
    SimStruct * S;
    int tid;
{
    int nOutputs;
    Matrix *quant;
    double *q;
    int size, i;
    
    nOutputs = ssGetNumOutputs(S);
    quant    = ssGetArg(S, 0);
    size     = mxGetM(quant) * mxGetN(quant);
    q        = mxGetPr(quant);
    
    if (size == 1) {
	for (i = 0; i < nOutputs; i++) {
	    y[i] = q[0] * floor(fabs(u[i]/q[0]) + 0.5) * (u[i] >= 0 ? 1.0 : -1.0);
	}
    } else {
	for (i = 0; i < nOutputs; i++) {
	    y[i] = q[i] * floor(fabs(u[i]/q[i]) + 0.5) * (u[i] >= 0 ? 1.0 : -1.0);
	}
    }
}

/*
 * mdlUpdate - perform action at major integration time step
 *
 * This function is called once for every major integration time step.
 * Discrete states are typically updated here, but this function is useful
 * for performing any tasks that should only take place once per integration
 * step.
 */

static void mdlUpdate(x, u, S, tid)
    double * x;
    double * u;
    SimStruct * S;
    int tid;
{
}

/*
 * mdlDerivatives - compute the derivatives
 *
 * In this function, you compute the S-function block's derivatives.
 * The derivatives are placed in the dx variable.
 */

static void mdlDerivatives(dx, x, u, S, tid)
    double * dx;
    double * x;
    double * u;
    SimStruct * S;
    int tid;
{
}

/*
 * mdlTerminate - called when the simulation is terminated.
 *
 * In this function, you should perform any actions that are necessary
 * at the termination of a simulation.  For example, if memory was allocated
 * in mdlInitializeConditions, this is the place to free it.
 */

static void mdlTerminate(S)
    SimStruct * S;
{
}

#ifdef	MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif
