/*
 *  dsp_ctrl_ts.c
 *
 *  Sample time "handshake" code for S-Functions with
 *  N-inputs (N>0) and N-outputs (N>0), where all input
 *  ports beyond the first one are"control" type inputs.
 *
 *  Output port rate(s) are set to the rate of input port 0.
 *  Second and subsequent input ports are "control" lines which must
 *   have rates that are integer multiples of input port 0.
 *
 *  Verify that sample times are discrete and discrete-offsets are zero.
 *
 *  Optional defines which affect behavior:
 *    DSP_ALLOW_CONTINUOUS
 *      Allow continuous sample times; otherwise generate
 *      an error if sample time is not discrete.
 *    DSP_ALLOW_MULTIPLE_TS
 *      Allow input ports to have a sample time that are
 *      multiples of the sample time of input port 0.
 *      Otherwise, the inputs must all have identical sample times.
 *
 *
 *  NOTE: S-function must use port-based sample times!
 *
 *  Typical usage:
 *    Instead of defining mdlInitializeSampleTimes(), etc., simply include:
 *
 *    #include "dsp_ctrl_ts.c"
 *
 *
 *  Author: D. Orofino
 *  Copyright (c) 1995-1999 The MathWorks, Inc. All Rights Reserved.
 *  $Revision: 1.14 $  $Date: 2000/04/20 13:36:03 $
 */

/*
 * Set all undefined ports to sampleTime and offsetTime.
 * Verify that all set output ports have sampleTime as the rate
 *   and offsetTime as the offset.
 */
static void setAllOutputPorts(SimStruct *S,
                       real_T sampleTime,
                       real_T offsetTime)
{
    const int_T numOutp = ssGetNumOutputPorts(S);
    int_T i;
    for (i=0; i<numOutp; i++) {
        if (ssGetOutputPortSampleTime(S, i) == INHERITED_SAMPLE_TIME) {
            ssSetOutputPortSampleTime(S, i, sampleTime);
            ssSetOutputPortOffsetTime(S, i, offsetTime);
        }
    }
}


/*
 *  Given a known port rate (portType, portIdx),
 *  set all undefined ports.
 */
static void setRates(SimStruct *S,
              int_T    portType,
              int_T    portIdx)
{
    if (portType==0) {
        /* in0 known */

        real_T sampleTime = ssGetInputPortSampleTime(S, portIdx);
        real_T offsetTime = ssGetInputPortOffsetTime(S, portIdx);
#ifdef MATLAB_MEX_FILE
        if (sampleTime == INHERITED_SAMPLE_TIME) {
            THROW_ERROR(S, "setRates: Given input port must have a non-inherited sample time.");
        }
        if (portIdx != 0) {
            THROW_ERROR(S, "setRates: May only be called for input port 0.");
        }
#endif

        /* Set all output ports, and check against previously-set output ports */
        setAllOutputPorts(S, sampleTime, offsetTime);

    } else {
        /* an output port is known */

        real_T sampleTime = ssGetOutputPortSampleTime(S, portIdx);
        real_T offsetTime = ssGetOutputPortOffsetTime(S, portIdx);
#ifdef MATLAB_MEX_FILE
        if (sampleTime == INHERITED_SAMPLE_TIME) {
            THROW_ERROR(S, "setRates: Given output port must have a non-inherited sample time.");
        }
#endif

        /* Set all output ports, and check against previously-set output ports */
        setAllOutputPorts(S, sampleTime, offsetTime);

        /* Set input port 0 */
        ssSetInputPortSampleTime(S,0,sampleTime);
        ssSetInputPortOffsetTime(S,0,offsetTime);
    }
}


static void setControlRates(SimStruct *S,
                   int_T     portType,
                   int_T     portIdx,
                   real_T    sampleTime,
                   real_T    offsetTime)
{
    const int_T numInp  = ssGetNumInputPorts(S);
    const int_T numOutp = ssGetNumOutputPorts(S);

#ifdef MATLAB_MEX_FILE
    if (ssGetNumSampleTimes(S) != PORT_BASED_SAMPLE_TIMES) {
        THROW_ERROR(S, "S-Function must use PORT_BASED_SAMPLE_TIMES in order "
                     "to be able to include dsp_ctrl_ts.");
    }
    if (numInp<1) {
        THROW_ERROR(S, "Must have at least 1 input port to use 'dsp_ctrl_ts.c'.");
    }
    if (numOutp<1) {
        THROW_ERROR(S, "Must have at least 1 output port to use 'dsp_ctrl_ts.c'.");
    }
#endif

    /*
     * Rules:
     *
     *  Outport rate = inport #1 rate.
     *  Inport #2 rate must be an integer multiple of
     *     inport #1 rate.
     */
    if (portType==0) { /* Inport */
        ssSetInputPortSampleTime(S, portIdx, sampleTime);
        ssSetInputPortOffsetTime(S, portIdx, offsetTime);

    } else {          /* Outport */
        ssSetOutputPortSampleTime(S, portIdx, sampleTime);
        ssSetOutputPortOffsetTime(S, portIdx, offsetTime);
    }

    if (numInp>1) {

        /* Only call the following "set & check" code for
         * output ports (portType=1), or input port 0:
         */
        if ((portType==1) || (portIdx==0)) {
            setRates(S, portType, portIdx);
        }
    } else {
        /* 1-input only:
         * Set all output ports to the same input0 rate as appropriate.
         */
        setAllOutputPorts(S, sampleTime, offsetTime);
    }
}


#define MDL_SET_INPUT_PORT_SAMPLE_TIME
static void mdlSetInputPortSampleTime(SimStruct *S,
                                      int_T     portIdx,
                                      real_T    sampleTime,
                                      real_T    offsetTime)
{
    setControlRates(S, 0, portIdx, sampleTime, offsetTime);
}


#define MDL_SET_OUTPUT_PORT_SAMPLE_TIME
static void mdlSetOutputPortSampleTime(SimStruct *S,
                                       int_T     portIdx,
                                       real_T    sampleTime,
                                       real_T    offsetTime)
{
    setControlRates(S, 1, portIdx, sampleTime, offsetTime);
}


static void CheckSampleTimes(SimStruct *S,
                      real_T sampleTime,
                      real_T offsetTime)
{
#ifdef MATLAB_MEX_FILE
    /* Check for continuous sample times: */
#ifndef DSP_ALLOW_CONTINUOUS
    if (sampleTime <= CONTINUOUS_SAMPLE_TIME) {
        THROW_ERROR(S, "Continuous and constant sample times not permitted.");
    }
#endif

    /* Check that offset=0 for discrete sample times: */
    if (sampleTime > CONTINUOUS_SAMPLE_TIME) {
        if (offsetTime != 0.0) {
            THROW_ERROR(S, "Non-zero offsets for discrete sample times not permitted.");
        }
    }
#endif
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    /* Check for consistency with pre-set sample time: */
    int_T  numOutPorts  = ssGetNumOutputPorts(S);
    int_T  numInPorts   = ssGetNumInputPorts(S);
    
    if (numInPorts > 0) {
        real_T sampleTime_i0 = ssGetInputPortSampleTime(S,0);
        real_T offsetTime_i0 = ssGetInputPortOffsetTime(S,0);
    
        /*
         * Check that input port 0 and all output ports have identical sample times:
         */
        
#ifdef MATLAB_MEX_FILE
        {
            int_T  i;
            
            for (i=0; i<numOutPorts; i++) {
                if ((sampleTime_i0 != ssGetOutputPortSampleTime(S,i)) ||
                    (offsetTime_i0 != ssGetOutputPortOffsetTime(S,i))) {
                    THROW_ERROR(S, "Sample time of first input port and all output ports must be identical.");
                }
            }
            
#ifdef DSP_ALLOW_MULTIPLE_TS
            /* Check to make sure all ports have continuous */
            /* and/or all port have discrete sample times.  */
            if ( !(areAllInputPortsSameBehaviorSampleTime(S) ) ) {
                THROW_ERROR(S, "Input ports must all have discrete sample times or continuous sample times.  Mixed sample-time behavior is not currently supported.");
            }
#endif
            
            /* 
            * Check that input port 0 and all additional input ports have
            * either identical discrete sample times, or all additional ports have
            * discrete sample times which are integer multiples input port 0.
            */
            if ( areAllInputPortsDiscreteSampleTime(S) ) {
#ifdef DSP_ALLOW_MULTIPLE_TS
                for (i=1; i<numInPorts; i++) {
                    real_T sampleTime = ssGetInputPortSampleTime(S,i);
                    real_T ratio = floor(sampleTime/sampleTime_i0 + 0.5);
                    
                    if ((ratio < 1.0) || (fabs(ratio - sampleTime/sampleTime_i0) > mxGetEps()*128.0)) {
                        THROW_ERROR(S, "Sample time of control inputs (input port 2 and higher) "
                            "must be positive multiples of the sample time "
                            "of the signal input (port 1).");
                    }
                }
#else
                for (i=1; i<numInPorts; i++) {
                    if ((sampleTime_i0 != ssGetInputPortSampleTime(S,i)) ||
                        (offsetTime_i0 != ssGetInputPortOffsetTime(S,i))) {
                        THROW_ERROR(S, "Sample time of control inputs (input port 2 and higher) "
                            "must be identical to the sample time "
                            "of the signal input (port 1).");
                    }
                }
#endif
            }
        }
#endif
    }
}

/* Undefine optional behavior flags: */
#ifdef DSP_ALLOW_CONTINUOUS
#   undef DSP_ALLOW_CONTINUOUS
#endif

#ifdef DSP_ALLOW_MULTIPLE_TS
#   undef DSP_ALLOW_MULTIPLE_TS
#endif

/* [EOF] dsp_control_ts.c */