// -*-c++-*-
// <mtt_model_name>_sfun.cc:
// Matlab S-function simulation of <mtt_model_name>

#define S_FUNCTION_NAME <mtt_model_name>_sfun
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"
#include <string>
#include "<mtt_model_name>_def.h"

static mxArray *P		= mxCreateDoubleMatrix (MTTNPAR	, 1, mxREAL);
static mxArray *T		= mxCreateDoubleMatrix (1	, 1, mxREAL);
static mxArray *U		= mxCreateDoubleMatrix (MTTNU	, 1, mxREAL);
static mxArray *X		= mxCreateDoubleMatrix (MTTNX	, 1, mxREAL);

static double *inputs		= mxGetPr (U);
static double *parameters	= mxGetPr (P);
static double *states		= mxGetPr (X);
static double *simtime		= mxGetPr (T);

void
update_parameters_from_simulink (SimStruct *S)
{
  for (int i = 0; i < MTTNPAR; i++) {
    parameters [i] = *mxGetPr (ssGetSFcnParam (S, i));
  }
}

void
update_states_from_simulink (SimStruct *S)
{
  static double *x;
  x = ssGetContStates (S);
  for (int i = 0; i < MTTNX; i++) {
    states [i] = x [i];
  }
}

void
update_inputs_from_simulink (SimStruct *S)
{
  for (int i = 0; i < MTTNU; i++) {
    inputs [i] = *ssGetInputPortRealSignalPtrs (S, i)[0];
  }
}

void
update_simtime_from_simulink (SimStruct *S)
{
  simtime [0] = ssGetT (S);
}

// S-function methods

static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, MTTNPAR); 
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        return;
    }

    ssSetNumContStates(S, MTTNX);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, MTTNU)) return;
    for (int i = 0; i < MTTNU; i++) {
      ssSetInputPortWidth(S, i, 1);
      ssSetInputPortDirectFeedThrough(S, i, 1);
    }

    if (!ssSetNumOutputPorts(S, MTTNY)) return;
    for (int i = 0; i < MTTNY; i++) {
      ssSetOutputPortWidth(S, i, 1);
    }

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}

static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}

#define MDL_INITIALIZE_CONDITIONS
static void mdlInitializeConditions(SimStruct *S)
{
  static mxArray *plhs[1];
  static mxArray *prhs[1];

  update_parameters_from_simulink (S);

  plhs[0] = X;  
  prhs[0] = P;
  
  mexCallMATLAB (1, plhs, 1, prhs, "<mtt_model_name>_state");  

  for (int i = 0; i < MTTNX; i++) {
    ssGetContStates (S)[i] = states [i];
  }
}

static void mdlOutputs(SimStruct *S, int_T tid)
{
  static mxArray *plhs[1];
  static mxArray *prhs[4];

  update_states_from_simulink (S);
  update_inputs_from_simulink (S);
  update_simtime_from_simulink (S);
  update_parameters_from_simulink (S);

  prhs[0] = X;
  prhs[1] = U;
  prhs[2] = T;
  prhs[3] = P;
 
  UNUSED_ARG(tid); // not used in single tasking mode

  mexCallMATLAB (1, plhs, 4, prhs, "<mtt_model_name>_odeo");
  double *outputs = mxGetPr (plhs[0]);

  for (int i = 0; i < MTTNY; i++) {
      ssGetOutputPortRealSignal (S,i)[0] = outputs [i];
  }
}

#define MDL_DERIVATIVES
static void mdlDerivatives(SimStruct *S)
{
  static mxArray *plhs[1];
  static mxArray *prhs[4];

  update_states_from_simulink (S);
  update_inputs_from_simulink (S);
  update_simtime_from_simulink (S);
  update_parameters_from_simulink (S);

  prhs[0] = X;
  prhs[1] = U;
  prhs[2] = T;
  prhs[3] = P;

  mexCallMATLAB (1, plhs, 4, prhs, "<mtt_model_name>_ode");
  double *rates= mxGetPr (plhs[0]);
  
  for (int i = 0; i < MTTNX; i++) {
    ssGetdX (S)[i] = rates [i];
  }
}

static void mdlTerminate(SimStruct *S)
{
    UNUSED_ARG(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
