/*  File    : schedule_event.c
 *  Abstract:
 *
 *
 *      For more details about S-functions, see simulink/src/sfuntmpl_doc.c.
 *
 *  Copyright 1990-2010 The MathWorks, Inc.
 *  $Revision: 1.1.6.2 $
 */

#define S_FUNCTION_NAME schedule_event
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"

#define N_IDX 0
#define N_PARAM(S) ssGetSFcnParam(S,N_IDX)

#define DELAY_IDX 1
#define DELAY_PARAM(S) ssGetSFcnParam(S,DELAY_IDX)

#define SEARCH_STEP_IDX 2
#define SEARCH_STEP_PARAM(S) ssGetSFcnParam(S,SEARCH_STEP_IDX)


/* Function: mdlInitializeSizes ===========================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    
    ssSetNumSFcnParams(S, 3);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        return; /* Parameter mismatch will be reported by Simulink */
    }
    
    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);
    
    if (!ssSetNumInputPorts(S, 1)) return;
    ssSetInputPortWidth(S, 0, 1);
    ssSetInputPortDataType(S, 0, SS_BOOLEAN);
    ssSetInputPortDirectFeedThrough(S, 0, 0);
    
    if (!ssSetNumOutputPorts(S, 1)) return;
    ssSetOutputPortWidth(S, 0, 1);
    
    ssSetNumSampleTimes(S, 2);
    
    /* buffer of N samples*/
    {
        const real_T      *Npr     = mxGetPr(N_PARAM(S));
        ssSetNumRWork(S, (int_T)Npr[0]+1);
    }
    
    /* index of the next event to be scheduled in the buffer */
    ssSetNumIWork(S, 2);
    
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
    
    if (ssGetSimMode(S) == SS_SIMMODE_RTWGEN && !ssIsVariableStepSolver(S)) {
        ssSetErrorStatus(S, "S-function vsfunc.c cannot be used with Simulink Coder "
                "and Fixed-Step Solvers because it contains variable"
                " sample time");
    }
    
}



/* Function: mdlInitializeSampleTimes =====================================
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, FIXED_IN_MINOR_STEP_OFFSET);
    ssSetSampleTime(S, 1, VARIABLE_SAMPLE_TIME);
    ssSetOffsetTime(S, 1, 0.0);
    
    ssSetCallSystemOutput(S,0);
}



#define MDL_INITIALIZE_CONDITIONS
/* Function: mdlInitializeConditions ======================================
 * Abstract:
 *    Initialize discrete state to zero.
 */
static void mdlInitializeConditions(SimStruct *S)
{
    int_T  i;
    real_T *rwork = ssGetRWork(S);
    const real_T      *Npr     = mxGetPr(N_PARAM(S));
    
    /* Initialize the buffer */
    for (i = 0; i <= (Npr[0]); i++) {
        rwork[i] = 0;
    }
}

/* Function: mdlGetTimeOfNextVarHit =======================================
 */
#define MDL_GET_TIME_OF_NEXT_VAR_HIT
static void mdlGetTimeOfNextVarHit(SimStruct *S)
{
    real_T *rwork = ssGetRWork(S);
    const real_T      *Npr     = mxGetPr(N_PARAM(S));
    const real_T      *SEARCH_STEP_pr     = mxGetPr(SEARCH_STEP_PARAM(S));
    int_T *n = ssGetIWork(S);
    
    /* empty buffer case */
    if (n[0] == 0){
        ssSetTNext(S, ssGetT(S)+SEARCH_STEP_pr[0]);
    }
    else{
        ssSetTNext(S,rwork[n[0]]);
        
        /* Decrease index of event to be scheduled*/
        n[0] = n[0]-1;
    }
}

/* Function: mdlUpdate ====================================================
 */
#define MDL_UPDATE
#if defined(MDL_UPDATE) && defined(MATLAB_MEX_FILE)
static void mdlUpdate(SimStruct *S, int_T tid)
{
    
    InputBooleanPtrsType u2Ptr = (InputBooleanPtrsType)
    ssGetInputPortSignalPtrs(S,0);
    int_T  i;
    real_T *rwork = ssGetRWork(S);
    const real_T      *Npr     = mxGetPr(N_PARAM(S));
    const real_T      *DELAYpr     = mxGetPr(DELAY_PARAM(S));
    int_T *n = ssGetIWork(S);
    
    /* Change is detected */
    if(*u2Ptr[0] && (ssGetT(S)>0.0)){
        
        /* Increase index of event to be scheduled*/
        n[0] = n[0]+1;
        /* Increase index of event to be generated*/
        n[1] = n[1]+1;
        
        /*Buffer overflow detection*/
        if ( (n[0]> Npr[0]) || (n[1]> Npr[0])){
            ssSetErrorStatus(S,"Buffer Overflow. Increase buffer size.");
        }
        
        /* Move the buffer and store the new time.*/
        for (i = Npr[0]; i >= 2; i--) {
            rwork[i] = rwork[i-1];
        }
        rwork[1] = ssGetT(S) + DELAYpr[0];
    }
}
#endif



/* Function: mdlOutputs ===================================================
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    real_T *rwork = ssGetRWork(S);
    const real_T      *Npr     = mxGetPr(N_PARAM(S));
    int_T *n = ssGetIWork(S);
    
    /* If this is a hit of the variable sample time
     * and current time is equal to time of next event to be outputted */
    if (ssIsSampleHit(S, 1, tid) && (n[1] != 0) && (rwork[n[1]] == ssGetT(S))  ) {
        ssCallSystemWithTid(S, 0, tid);
        /* Decrease index of event to be generated*/
        n[1] = n[1]-1;
    }
}





/* Function: mdlTerminate =================================================
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(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