#if !defined(S_FUNCTION_NAME)
#define S_FUNCTION_NAME                 Controller_sf
#endif
#define S_FUNCTION_LEVEL                2

#include <stdio.h>
#include <math.h>
#include "simstruc.h"
#include "fixedpoint.h"
#include "rtlibsrc.h"

#define rt_logging_h
#define RTWSfcnInfo                     void *
#include "Controller.h"

static int8_T Controller_sf_counter = 0;

#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
static void mdlCheckParameters(SimStruct *S)
{
  
  {
    int dimsArray[2] = {1, 1};
    ssCheckSFcnParamValueAttribs(S, 0, "FeedForwardGain_Gain", DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {16, 1};
    ssCheckSFcnParamValueAttribs(S, 1, "Observer_A", DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {8, 1};
    ssCheckSFcnParamValueAttribs(S, 2, "Observer_B", DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {4, 1};
    ssCheckSFcnParamValueAttribs(S, 3, "Observer_C", DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {1, 1};
    ssCheckSFcnParamValueAttribs(S, 4, "Observer_X0", DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {1, 4};
    ssCheckSFcnParamValueAttribs(S, 5, "L2_Gain", DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {1, 1};
    ssCheckSFcnParamValueAttribs(S, 6, "DiscreteTimeIntegrator_g",
     DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {1, 1};
    ssCheckSFcnParamValueAttribs(S, 7, "DiscreteTimeIntegrator_I",
     DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }

  {
    int dimsArray[2] = {1, 1};
    ssCheckSFcnParamValueAttribs(S, 8, "IntegralGain_Gain", DYNAMICALLY_TYPED,
     2, dimsArray, false);
  }
}
#endif

static void updateGlobalVarsFromRunTimeParams(SimStruct *S)
{
  int_T i;

  Controller_P.FeedForwardGain_Gain = (((real_T *)
    ssGetRunTimeParamInfo(S,0)->data)[0]);

  for (i=0; i<16; i++) {
    Controller_P.Observer_A[i] = (((real_T *)
      ssGetRunTimeParamInfo(S,1)->data)[i]);
  }

  for (i=0; i<8; i++) {
    Controller_P.Observer_B[i] = (((real_T *)
      ssGetRunTimeParamInfo(S,2)->data)[i]);
  }

  for (i=0; i<4; i++) {
    Controller_P.Observer_C[i] = (((real_T *)
      ssGetRunTimeParamInfo(S,3)->data)[i]);
  }

  Controller_P.Observer_X0 = (((real_T *) ssGetRunTimeParamInfo(S,4)->data)[0]);

  for (i=0; i<4; i++) {
    Controller_P.L2_Gain[i] = (((real_T *) ssGetRunTimeParamInfo(S,5)->data)[i]);
  }

  Controller_P.DiscreteTimeIntegrator_g = (((real_T *)
    ssGetRunTimeParamInfo(S,6)->data)[0]);

  Controller_P.DiscreteTimeIntegrator_I = (((real_T *)
    ssGetRunTimeParamInfo(S,7)->data)[0]);

  Controller_P.IntegralGain_Gain = (((real_T *)
    ssGetRunTimeParamInfo(S,8)->data)[0]);
}

#define MDL_PROCESS_PARAMETERS
#if defined(MDL_PROCESS_PARAMETERS) && defined(MATLAB_MEX_FILE)
static void mdlProcessParameters(SimStruct *S)
{
  ssUpdateAllTunableParamsAsRunTimeParams(S);
  updateGlobalVarsFromRunTimeParams(S);
}
#endif

static void mdlInitializeSizes(SimStruct *S)
{
  
  ssSetNumSFcnParams(S, 9);
  
  if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
#if defined(MDL_CHECK_PARAMETERS)
    mdlCheckParameters(S);
#endif
    if (ssGetErrorStatus(S) != NULL) {
      return;
    }
  } else {
    return;
  }

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

  if (!ssSetNumInputPorts(S, 2)) return;
  if(!ssSetInputPortVectorDimension(S, 0, 3)) return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetInputPortDataType(S, 0, SS_DOUBLE);
  }

  ssSetBusInputAsStruct(S, 0, 1);
  ssSetInputPortDirectFeedThrough(S, 0, 1);
  ssSetInputPortSampleTime(S, 0, 0.0001);
  ssSetInputPortOffsetTime(S, 0, 0.0);
  if(!ssSetInputPortVectorDimension(S, 1, 1)) return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetInputPortDataType(S, 1, SS_DOUBLE);
  }

  ssSetBusInputAsStruct(S, 1, 1);
  ssSetInputPortDirectFeedThrough(S, 1, 1);
  ssSetInputPortSampleTime(S, 1, 0.0001);
  ssSetInputPortOffsetTime(S, 1, 0.0);

  if (!ssSetNumOutputPorts(S, 1)) return;

  if(!ssSetOutputPortVectorDimension(S, 0, 1)) return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 0, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 0, 0.0001);
  ssSetOutputPortOffsetTime(S, 0, 0.0);

  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, 0.0001);
  ssSetOffsetTime(S, 0, 0.0);
}

#define MDL_SET_WORK_WIDTHS
#if defined(MDL_SET_WORK_WIDTHS) && defined(MATLAB_MEX_FILE)
static void mdlSetWorkWidths(SimStruct *S)
{

  if (!ssSetNumRunTimeParams(S, 9)) return;

  ssRegDlgParamAsRunTimeParam(S, 0, 0, "P0_FeedForwardGain_Gain", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 1, 1, "P1_Observer_A", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 2, 2, "P2_Observer_B", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 3, 3, "P3_Observer_C", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 4, 4, "P4_Observer_X0", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 5, 5, "P5_L2_Gain", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 6, 6, "P6_DiscreteTimeIntegrator_g", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 7, 7, "P7_DiscreteTimeIntegrator_I", SS_DOUBLE);

  ssRegDlgParamAsRunTimeParam(S, 8, 8, "P8_IntegralGain_Gain", SS_DOUBLE);

  updateGlobalVarsFromRunTimeParams(S);
}
#endif

#define MDL_INITIALIZE_CONDITIONS

static void mdlInitializeConditions(SimStruct *S)
{

#ifdef PIL_S_FUNCTION
  pilMarshallInitSFcnSimStruct(S);
  pilMarshallInitRootSimStruct(ssGetRootSS(S));
#endif

  Controller_initialize(1);
}

#define MDL_START
#if defined(MDL_START)

static void mdlStart(SimStruct *S)
{
  
  if (++Controller_sf_counter > 1) {
    ssSetErrorStatus(S,"This S-Function is limited to one copy per model.");
    Controller_sf_counter = 0;
    return;
  }
}
#endif

static void mdlOutputs(SimStruct *S, int_T tid)
{
  InputRealPtrsType InPort_0 = (InputRealPtrsType) ssGetInputPortSignalPtrs(S,
   0);

  InputRealPtrsType InPort_1 = (InputRealPtrsType) ssGetInputPortSignalPtrs(S,
   1);

  real_T *OutPort_0 = (real_T *)ssGetOutputPortSignal(S, 0);

  Controller_U.trajectory[0] = InPort_0[0][0];
  Controller_U.trajectory[1] = InPort_0[1][0];
  Controller_U.trajectory[2] = InPort_0[2][0];

  Controller_U.Encoder = InPort_1[0][0];

  Controller_step();

  OutPort_0[0] = Controller_Y.U;
}

static void mdlTerminate(SimStruct *S)
{

  Controller_sf_counter = 0;
}

#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#include "fixedpoint.c"
#else
#include "cg_sfun.h"
#endif