/*
 * VDLMINTC  Example MEX-file for a discrete-time vectorized limited integrator.
 *           (see manual under Advanced Topics).
 *
 *           Syntax:  [sys, x0] = vdlmintc(t,x,u,flag,lb,ub,xi,ts)
 * 
 *  Rick Spada 9-3-93
 *  Copyright (c) 1993-94 by The MathWorks, Inc.
 *  All Rights Reserved
 */

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

#define S_FUNCTION_NAME vdlmintc

#ifdef MATLAB_MEX_FILE
#include "mex.h"      /* needed for declaration of mexErrMsgTxt */
#endif

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

#include "simstruc.h"

/*
 * Defines for easy access of the LB, UB, X0 matrices which are passed in
 */

#define LB	ssGetArg(S,0)
#define UB	ssGetArg(S,1)
#define X0	ssGetArg(S,2)
#define TS	ssGetArg(S,3)

#define GetNumEl(m)  (mxGetM(m)*mxGetN(m))


/*
 * 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;
{
    int sizeLB;
    
    ssSetNumContStates(    S, 0);                  /* number of continuous states */
    ssSetNumDiscStates(    S, DYNAMICALLY_SIZED);  /* number of discrete states */
    ssSetNumInputs(        S, DYNAMICALLY_SIZED);  /* number of inputs */
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED);  /* number of outputs */
    ssSetDirectFeedThrough(S, 0);                  /* direct feedthrough flag */
    ssSetNumSampleTimes(   S, 1);                  /* number of sample times */
    ssSetNumInputArgs(     S, 4);                  /* 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 */
    
    /*
     * if there aren't four parameters, just return, simulink.c will
     * flag the error
     */
    
    if (ssGetNumArgs(S) != 4)
	return;
    
    /*
     * if the parameter matrices are not all vectors of the same length
     * flag an error
     */
    
    if (((mxGetN(LB) != 1) || (mxGetM(LB) != 1)) ||
	((mxGetM(UB) != 1) || (mxGetM(UB) != 1)) ||
	((mxGetM(X0) != 1) || (mxGetM(X0) != 1))) {
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt("The parameters must be vectors");
#endif
    }
    
    sizeLB = GetNumEl(LB);
    if ((sizeLB != GetNumEl(UB)) || (sizeLB != GetNumEl(X0))) {
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt("The parameters lengths do not match");
#endif
    }
    
    if (sizeLB == 0) {
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt("Empty matrices not allowed");
#endif
    }
    
    if (GetNumEl(TS) != 1) {
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt("Sample time must be scalar");
#endif
    }
    
    /*
     * if the length of the parameters is not scalar, fix
     * the input, output, and states width
     */
    
    if (sizeLB > 1) {
	ssSetNumDiscStates(S, sizeLB);
	ssSetNumInputs(    S, sizeLB);
	ssSetNumOutputs(   S, sizeLB);
    }
}

/*
 * 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.
 */

static void mdlInitializeSampleTimes(S)
    SimStruct *S;
{
    ssSetSampleTimeEvent (S, 0, mxGetPr(TS)[0]);
    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;
{
    int i, sizeX0;
    double *pX0;
    
    /*
     * all that has to be done here is to return the n initial
     * conditions in the X0 vector.  If this is a scalar, and the
     * number of states is not 1, SIMULINK automatically propogates
     * the initial condition to all states.  If this is a vector,
     * then all that needs to be done is to copy it to the x0 array,
     * since the number of states will be equal to the length of
     * this vector.
     */
    
    pX0 = mxGetPr(X0);
    sizeX0 = GetNumEl(X0);
    for (i = 0; i < sizeX0; i++) {
	*x0++ = *pX0++;
    }
}

/*
 * 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, *x, *u;
    SimStruct *S;
    int tid;
{
    /*
     * Propagate the discrete state to the output vector
     */
    
    int nOutputs = ssGetNumOutputs(S);
    int i;
	
    for (i = 0; i < nOutputs; i++)
      *y++ = *x++;
}

/*
 * 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, *u;
    SimStruct *S;
    int tid;
{
    double *lb, *ub;
    double ts;
    int sizeLB, nStates;
    int i;
	

    /*
     * If this is a sample hit, update the discrete states (i.e.
     * perform the discrete integration of the input signal)
     */
    
    /*
     * need the sample time
     */
    
    ts = mxGetPr(TS)[0];
    
    /*
     * and the upper and lower bounds
     */
    
    lb = mxGetPr(LB);
    ub = mxGetPr(UB);
    sizeLB = GetNumEl(LB);
    nStates = ssGetNumDiscStates(S);
    
    /*
     * If the parameters are scalars, the apply bounds to all states;
     * otherwise, apply individual bounds to each state.
     */
    
    if (sizeLB == 1) {
      for (i = 0; i < nStates; i++, x++, u++) {
	if ((*x <= *lb && *u < 0.0) || (*x >= *ub && *u > 0.0)) {
	  ;
	} else {
	  *x += *u * ts;
	}
      }
    } else {
      for (i = 0; i < nStates; i++, x++, u++, lb++, ub++) {
	if ((*x <= *lb && *u < 0.0) || (*x >= *ub && *u > 0.0)) {
	  ;
	} else {
	  *x += *u * ts;
	}
      }
    }

}

/*
 * 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, *x, *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
