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

#define S_FUNCTION_NAME <mtt_model_name>_sfun
#define S_FUNCTION_LEVEL 2

#if (0)				/* DEBUG? */
#define PRINT_ENTER(f) fprintf (stderr, "Entered %s\n", f)
#define PRINT_LEAVE(f) fprintf (stderr, "Leaving %s\n", f)
#else
#define PRINT_ENTER(f)
#define PRINT_LEAVE(f)
#endif /* DEBUG? */

#include <stdio.h>
#include <stdlib.h>

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

static double *mttdx;		/* pointer to rates */
static double *mttu;		/* pointer to inputs */
static double *mttpar;		/* pointer to parameters */
static double *mttx;		/* pointer to states */
static double *mtty;		/* pointer to outputs */
static double *mttyz;		/* pointer to residuals */
static double  mttt;		/* time */

static unsigned int i;		/* loop counter */

/* system equations */

static void
<mtt_model_name>_ae (void)
{
#include "<mtt_model_name>_ae.c"
}

static void
<mtt_model_name>_ode (void)
{
#include "<mtt_model_name>_ode.c"
}

static void
<mtt_model_name>_odeo (void)
{
#include "<mtt_model_name>_odeo.c"
}

static void
<mtt_model_name>_state (void)
{
#include "<mtt_model_name>_state.c"
}

/* utility procedures */

static double *
array_of_double (size_t n)
{
  void *p = calloc (n, sizeof (double));
  if (! p) {
    fprintf (stderr, "*** Error: failed to allocate memory\n");
  }
  return (double *) p;
}

static void
initialise_arrays (void)
{
  PRINT_ENTER("initialise_arrays");

  mttdx		= array_of_double (MTTNX);
  mttpar	= array_of_double (MTTNPAR);
  mttu		= array_of_double (MTTNU + MTTNYZ);
  mttx		= array_of_double (MTTNX);
  mtty		= array_of_double (MTTNY);
  mttyz		= array_of_double (MTTNYZ);

  PRINT_LEAVE("initialise_arrays");
}

static void
update_states_from_simulink (SimStruct *S)
{
  PRINT_ENTER("update_states_from_simulink");
  for (i = 0; i < MTTNX; i++) {
    mttx[i] = ssGetContStates (S)[i];
  }
  PRINT_LEAVE("update_states_from_simulink");
}

static void
update_inputs_from_simulink (SimStruct *S)
{
  PRINT_ENTER("update_inputs_from_simulink");
  for (i = 0; i < MTTNU; i++) {
    mttu[i] = *ssGetInputPortRealSignalPtrs (S, i)[0];
  }
  PRINT_LEAVE("update_inputs_from_simulink");
}

static void
update_inputs_from_solver (void)
{
  mxArray *MTT_MATLAB_P;
  mxArray *MTT_MATLAB_T;
  mxArray *MTT_MATLAB_U;
  mxArray *MTT_MATLAB_Ui;
  mxArray *MTT_MATLAB_X;

  double *p;

  PRINT_ENTER ("update_inputs_from_solver");

  /* starting value for solver - start with zero */
  MTT_MATLAB_Ui = mxCreateDoubleMatrix (MTTNYZ, 1, mxREAL);
  mxSetName (MTT_MATLAB_Ui, "MTT_Ui");
  p = mxGetPr (MTT_MATLAB_Ui);
  for (i = 0; i < MTTNYZ; i++) {
    p[i] = 0.0;
  }
  mexPutArray (MTT_MATLAB_Ui, "base");

  /* put states into matlab workspace */
  MTT_MATLAB_X = mxCreateDoubleMatrix (MTTNX, 1, mxREAL);
  mxSetName (MTT_MATLAB_X, "MTT_X");
  p = mxGetPr (MTT_MATLAB_X);
  for (i = 0; i < MTTNX; i++) {
    p[i] = mttx[i];
  }
  mexPutArray (MTT_MATLAB_X, "base");

  /* put known inputs into matlab workspace */
  MTT_MATLAB_U = mxCreateDoubleMatrix (MTTNU, 1, mxREAL);
  mxSetName (MTT_MATLAB_U, "MTT_U");
  p = mxGetPr (MTT_MATLAB_U);
  for (i = 0; i < MTTNU; i++) {
    p[i] = mttu[i];
  }
  mexPutArray (MTT_MATLAB_U, "base");

  /* put time into matlab workspace */
  MTT_MATLAB_T = mxCreateDoubleMatrix (1, 1, mxREAL);
  mxSetName (MTT_MATLAB_T, "MTT_T");
  *mxGetPr (MTT_MATLAB_T) = mttt;
  mexPutArray (MTT_MATLAB_T, "base");

  /* put parameters into matlab workspace */
  MTT_MATLAB_P = mxCreateDoubleMatrix (MTTNPAR, 1, mxREAL);
  mxSetName (MTT_MATLAB_P, "MTT_P");
  p = mxGetPr (MTT_MATLAB_P);
  for (i = 0; i < MTTNPAR; i++) {
    p[i] = mttpar[i];
  }
  mexPutArray (MTT_MATLAB_P, "base");

  /* call fsolve */
  mexEvalString ("MTT_Ui = fsolve (@<mtt_model_name>_sfun_ae, MTT_Ui, optimset('display','off'), MTT_X, MTT_U, MTT_T, MTT_P);");

  /* retrieve result */
  MTT_MATLAB_Ui = mexGetArray ("MTT_Ui", "base");
  p = mxGetPr (MTT_MATLAB_Ui);
  for (i = 0; i < MTTNYZ; i++) {
    mttu[MTTNU + i] = p[i];
  }

  /* free memory */
  mxDestroyArray (MTT_MATLAB_P);
  mxDestroyArray (MTT_MATLAB_T);
  mxDestroyArray (MTT_MATLAB_U);
  mxDestroyArray (MTT_MATLAB_Ui);
  mxDestroyArray (MTT_MATLAB_X);

  PRINT_LEAVE ("update_inputs_from_solver");
}

static void
update_simtime_from_simulink (SimStruct *S)
{
  PRINT_ENTER("update_simtime_from_simulink");
  mttt = ssGetT (S);
  PRINT_LEAVE("update_simtime_from_simulink");
}

/* S-function methods */

static void mdlInitializeSizes(SimStruct *S)
{
  PRINT_ENTER("mdlInitializeSizes");
  ssSetNumSFcnParams(S, 0); 
  if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
    PRINT_LEAVE("mdlInitializeSizes");
    return;
  }
  
  ssSetNumContStates(S, MTTNX);
  ssSetNumDiscStates(S, 0);
  
  if (!ssSetNumInputPorts(S, MTTNU)) return;
    for (i = 0; i < MTTNU; i++) {
      ssSetInputPortWidth(S, i, 1);
      ssSetInputPortDirectFeedThrough(S, i, 1);
    }
    
    if (!ssSetNumOutputPorts(S, MTTNY)) return;
    for (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);
    
    initialise_arrays ();
    PRINT_LEAVE("mdlInitializeSizes");
}

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

#define MDL_INITIALIZE_CONDITIONS
static void mdlInitializeConditions(SimStruct *S)
{
#include "<mtt_model_name>_sympar.h"

  PRINT_ENTER("mdlInitializeConditions");

#include "<mtt_model_name>_numpar.c"

  <mtt_model_name>_state ();

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

  PRINT_LEAVE("leaving mdlInitializeConditions");
}

static void mdlOutputs(SimStruct *S, int_T tid)
{
  PRINT_ENTER("entered mdlOutputs");

  update_states_from_simulink (S);
  update_inputs_from_simulink (S);
  if (MTTNYZ > 0) {
    update_inputs_from_solver ();
  }
  update_simtime_from_simulink (S);

  UNUSED_ARG(tid); /* not used in single tasking mode */

  <mtt_model_name>_odeo ();

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

  PRINT_LEAVE("leaving mdlOutputs");
}

#define MDL_DERIVATIVES
static void mdlDerivatives(SimStruct *S)
{
  PRINT_ENTER("entered mdlDerivatives\n");
    
  update_states_from_simulink (S);
  update_inputs_from_simulink (S);
  if (MTTNYZ > 0) {
    update_inputs_from_solver ();
  } 
  update_simtime_from_simulink (S);

  <mtt_model_name>_ode ();

  for (i = 0; i < MTTNX; i++) {
    ssGetdX (S)[i] = mttdx[i];
  }

  PRINT_LEAVE("leaving mdlDerivatives");
}

static void mdlTerminate(SimStruct *S)
{
  PRINT_ENTER("entered mdlTerminate");
  UNUSED_ARG(S);

  free (mttdx);
  free (mttpar);
  free (mttu);
  free (mttx);
  free (mtty);
  free (mttyz);

  PRINT_LEAVE("leaving mdlTerminate");
}

#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
