#! /usr/bin/make -f

$(SYS)_sfun.mexglx: $(SYS)_sfun.cc $(SYS)_def.h $(SYS)_state.mexglx $(SYS)_ode.mexglx $(SYS)_odeo.mexglx
	cp -a $(SYS)_ode.mexglx ..
	cp -a $(SYS)_odeo.mexglx ..
	cp -a $(SYS)_state.mexglx ..
	mex $(SYS)_sfun.cc

$(SYS)_sfun.cc:: ${MTT_REP}/sfun_rep/sfun.cc.tmpl
	cat $^ | sed 's/<mtt_model_name>/$(SYS)/g' > $@

%::
	mtt -q $(OPTS) `echo $* | sed 's/\(.*\)_\(.*\)\.\(.*\)/\1 \2 \3/'`

// -*-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 <iostream>
#include <string>
#include "<mtt_model_name>_def.h"

mxArray *
get_parameters_from_simulink (SimStruct *S)
{
  static const mxArray *constpar = mxCreateDoubleMatrix (MTTNPAR, 1, mxREAL);
  static       mxArray *     par = mxCreateDoubleMatrix (MTTNPAR, 1, mxREAL);
  double *constptr;
  double *ptr;
  std::cerr << "Parameters:\t";
  for (int i = 0; i < MTTNPAR; i++) {
    constpar = ssGetSFcnParam (S, i);
    constptr = mxGetPr (constpar);
    ptr      = mxGetPr (     par);
    *ptr     = *constptr;
    std::cerr << *ptr << ' ';
  }
  std::cerr << std::endl;
  return par;
}

mxArray *
get_states_from_simulink (SimStruct *S)
{
  static mxArray *x = mxCreateDoubleMatrix (MTTNX, 1, mxREAL);
  static double *states;
  static double *ptr = mxGetPr (x);
  std::cerr << "States:\t\t";
  states = ssGetContStates (S);
  for (int i = 0; i < MTTNX; i++) {
    ptr [i] = states [i];
    std::cerr << ptr [i] << ' ';
  }
  std::cerr << std::endl;
  return x;
}

mxArray *
get_inputs_from_simulink (SimStruct *S)
{
  static mxArray *u = mxCreateDoubleMatrix (MTTNU, 1, mxREAL);
  static double *ptr = mxGetPr (u);
  std::cerr << "Inputs:\t\t";
  for (int i = 0; i < MTTNU; i++) {
    ptr [i] = *ssGetInputPortRealSignalPtrs (S, i)[0];
    std::cerr << ptr [i] << ' ';
  }
  std::cerr << std::endl;
  return u;
}

mxArray *
get_time_from_simulink (SimStruct *S)
{
  static mxArray *t = mxCreateDoubleMatrix (1, 1, mxREAL);
  double *ptr = mxGetPr (t);
  ptr [0] = ssGetT (S);
  std::cerr << "Time:\t\t" << ptr [0] << std::endl;
  return t;
}

// 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, 1)) return;
    ssSetInputPortWidth(S, 0, 1);
    for (int i = 0; i < MTTNU; i++) {
      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 *states = mxCreateDoubleMatrix (MTTNX, 1, mxREAL);
  static double *ptr;

  mxArray *plhs[1];
  mxArray *prhs[1];

  plhs[0] = states;
  
  prhs[0] = get_parameters_from_simulink (S);
  
  mexCallMATLAB (1, plhs, 1, prhs, "<mtt_model_name>_state");  
  ptr = mxGetPr (plhs[0]);

  std::cerr << "Initial states;\t";
  for (int i = 0; i < MTTNX; i++) {
    ssGetContStates (S)[i] = ptr [i];
    std::cerr << ptr [i] << ' ';
  }
  std::cerr << std::endl;
}

static void mdlOutputs(SimStruct *S, int_T tid)
{
  static mxArray *outputs = mxCreateDoubleMatrix (MTTNY, 1, mxREAL);
  static double *ptr;

  mxArray *plhs[1];
  mxArray *prhs[4];

  plhs[0] = outputs;

  prhs[0] = get_states_from_simulink (S);
  prhs[1] = get_inputs_from_simulink (S);
  prhs[2] = get_time_from_simulink (S);
  prhs[3] = get_parameters_from_simulink (S);
 
  UNUSED_ARG(tid); // not used in single tasking mode

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

  std::cerr << "Outputs:\t";
  for (int i = 0; i < MTTNY; i++) {
      ssGetOutputPortRealSignal (S,i)[0] = ptr [i];
      std::cerr << ptr [i] << ' ';
  }
  std::cerr << std::endl;
}

#define MDL_DERIVATIVES
static void mdlDerivatives(SimStruct *S)
{
  static mxArray *rates = mxCreateDoubleMatrix (MTTNX, 1, mxREAL);
  static double *ptr;

  mxArray *plhs[1];
  mxArray *prhs[4];

  plhs[0] = rates;

  prhs[0] = get_states_from_simulink (S);
  prhs[1] = get_inputs_from_simulink (S);
  prhs[2] = get_time_from_simulink (S);
  prhs[3] = get_parameters_from_simulink (S);

  mexCallMATLAB (1, plhs, 4, prhs, "<mtt_model_name>_ode");
  ptr = mxGetPr (plhs[0]);

  std::cerr << "Rates:\t\t";
  for (int i = 0; i < MTTNX; i++) {
    ssGetdX (S)[i] = ptr [i];
    std::cerr << ptr[i] << ' ';
  }
  std::cerr << std::endl;
}

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