/*
 * PMSM_Motor_TL3_sf.c
 *
 * Code generation for model "PMSM_Motor_TL3_sf".
 *
 * Model version              : 1.825
 * Simulink Coder version : 9.4 (R2020b) 29-Jul-2020
 * C source code generated on : Fri Apr 14 12:51:02 2023
 *
 * Target selection: rtwsfcn.tlc
 * Note: GRT includes extra infrastructure and instrumentation for prototyping
 * Embedded hardware selection: ARM Compatible->ARM Cortex-M
 * Emulation hardware selection:
 *    Differs from embedded hardware (MATLAB Host)
 * Code generation objectives:
 *    1. Execution efficiency
 *    2. RAM efficiency
 * Validation result: Not run
 */

#include <math.h>
#include "PMSM_Motor_TL3_sf.h"
#include "PMSM_Motor_TL3_sf_private.h"
#include "simstruc.h"
#include "fixedpoint.h"
#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)

extern void *PMSM_Motor_TL3_malloc(SimStruct *S);

#endif

#ifndef __RTW_UTFREE__
#if defined (MATLAB_MEX_FILE)

extern void * utMalloc(size_t);
extern void utFree(void *);

#endif
#endif                                 /* #ifndef __RTW_UTFREE__ */

/* Forward declaration for local functions */
static real_T PMSM_Motor_TL3_rt_atan2d_snf(real_T u0, real_T u1);

#if defined(MATLAB_MEX_FILE)
#include "rt_nonfinite.c"
#endif

static const char_T *RT_MEMORY_ALLOCATION_ERROR =
  "memory allocation error in generated S-Function";
static real_T PMSM_Motor_TL3_rt_atan2d_snf(real_T u0, real_T u1)
{
  real_T y;
  int32_T u0_0;
  int32_T u1_0;
  if (rtIsNaN(u0) || rtIsNaN(u1)) {
    y = (rtNaN);
  } else if (rtIsInf(u0) && rtIsInf(u1)) {
    if (u0 > 0.0) {
      u0_0 = 1;
    } else {
      u0_0 = -1;
    }

    if (u1 > 0.0) {
      u1_0 = 1;
    } else {
      u1_0 = -1;
    }

    y = atan2(u0_0, u1_0);
  } else if (u1 == 0.0) {
    if (u0 > 0.0) {
      y = RT_PI / 2.0;
    } else if (u0 < 0.0) {
      y = -(RT_PI / 2.0);
    } else {
      y = 0.0;
    }
  } else {
    y = atan2(u0, u1);
  }

  return y;
}

/* System initialize for root system: '<Root>' */
#define MDL_INITIALIZE_CONDITIONS

static void mdlInitializeConditions(SimStruct *S)
{
  if (ssIsFirstInitCond(S)) {
    B_PMSM_Motor_TL3_T *_rtB;
    _rtB = ((B_PMSM_Motor_TL3_T *) ssGetLocalBlockIO(S));

    /* InitializeConditions for DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' */
    ((real_T *)ssGetDWork(S, 0))[0] = 1.5707963267948966;

    /* InitializeConditions for DiscreteIntegrator: '<S18>/Discrete-Time Integrator' */
    ((real_T *)ssGetDWork(S, 1))[0] = 0.0;

    /* InitializeConditions for DiscreteIntegrator: '<S17>/Discrete-Time Integrator' */
    ((real_T *)ssGetDWork(S, 2))[0] = 0.0;

    /* InitializeConditions for S-Function (sfun_spssw_discc): '<S20>/State-Space' incorporates:
     *  Constant: '<S19>/Constant'
     */
    {
      int32_T i, j;
      real_T *Ds = (real_T*)((void**) ssGetDWork(S, 4))[3];

      /* Copy and transpose D */
      for (i=0; i<9; i++) {
        for (j=0; j<9; j++)
          Ds[i*9 + j] = (PMSM_Motor_TL3_ConstP.StateSpace_DS_param[i + j*9]);
      }

      {
        /* Switches work vectors */
        int_T *switch_status = (int_T*) ((void**) ssGetDWork(S, 4))[9];
        int_T *gState = (int_T*)((void**) ssGetDWork(S, 4))[12];
        real_T *yswitch = (real_T*)((void**) ssGetDWork(S, 4))[19];
        int_T *switchTypes = (int_T*)((void**) ssGetDWork(S, 4))[20];
        int_T *idxOutSw = (int_T*)((void**) ssGetDWork(S, 4))[21];
        int_T *switch_status_init = (int_T*) ((void**) ssGetDWork(S, 4))[10];

        /* Initialize work vectors */
        switch_status[0] = 0;
        switch_status_init[0] = 0;
        gState[0] = (int_T) 0.0;
        yswitch[0] = 1/0.001;
        switchTypes[0] = (int_T)7.0;
        idxOutSw[0] = ((int_T)0.0) - 1;
        switch_status[1] = 0;
        switch_status_init[1] = 0;
        gState[1] = (int_T) 0.0;
        yswitch[1] = 1/0.001;
        switchTypes[1] = (int_T)7.0;
        idxOutSw[1] = ((int_T)0.0) - 1;
        switch_status[2] = 0;
        switch_status_init[2] = 0;
        gState[2] = (int_T) 0.0;
        yswitch[2] = 1/0.001;
        switchTypes[2] = (int_T)7.0;
        idxOutSw[2] = ((int_T)0.0) - 1;
        switch_status[3] = 0;
        switch_status_init[3] = 0;
        gState[3] = (int_T) 0.0;
        yswitch[3] = 1/0.001;
        switchTypes[3] = (int_T)7.0;
        idxOutSw[3] = ((int_T)0.0) - 1;
        switch_status[4] = 0;
        switch_status_init[4] = 0;
        gState[4] = (int_T) 0.0;
        yswitch[4] = 1/0.001;
        switchTypes[4] = (int_T)7.0;
        idxOutSw[4] = ((int_T)0.0) - 1;
        switch_status[5] = 0;
        switch_status_init[5] = 0;
        gState[5] = (int_T) 0.0;
        yswitch[5] = 1/0.001;
        switchTypes[5] = (int_T)7.0;
        idxOutSw[5] = ((int_T)0.0) - 1;
      }
    }

    /* InitializeConditions for UnitDelay: '<S4>/Unit Delay' */
    ((real_T *)ssGetDWork(S, 3))[0] = 0.0;
  } else {
    B_PMSM_Motor_TL3_T *_rtB;
    _rtB = ((B_PMSM_Motor_TL3_T *) ssGetLocalBlockIO(S));

    /* InitializeConditions for DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' */
    ((real_T *)ssGetDWork(S, 0))[0] = 1.5707963267948966;

    /* InitializeConditions for DiscreteIntegrator: '<S18>/Discrete-Time Integrator' */
    ((real_T *)ssGetDWork(S, 1))[0] = 0.0;

    /* InitializeConditions for DiscreteIntegrator: '<S17>/Discrete-Time Integrator' */
    ((real_T *)ssGetDWork(S, 2))[0] = 0.0;

    /* InitializeConditions for S-Function (sfun_spssw_discc): '<S20>/State-Space' incorporates:
     *  Constant: '<S19>/Constant'
     */
    {
      int32_T i, j;
      real_T *Ds = (real_T*)((void**) ssGetDWork(S, 4))[3];

      /* Copy and transpose D */
      for (i=0; i<9; i++) {
        for (j=0; j<9; j++)
          Ds[i*9 + j] = (PMSM_Motor_TL3_ConstP.StateSpace_DS_param[i + j*9]);
      }

      {
        /* Switches work vectors */
        int_T *switch_status = (int_T*) ((void**) ssGetDWork(S, 4))[9];
        int_T *gState = (int_T*)((void**) ssGetDWork(S, 4))[12];
        real_T *yswitch = (real_T*)((void**) ssGetDWork(S, 4))[19];
        int_T *switchTypes = (int_T*)((void**) ssGetDWork(S, 4))[20];
        int_T *idxOutSw = (int_T*)((void**) ssGetDWork(S, 4))[21];
        int_T *switch_status_init = (int_T*) ((void**) ssGetDWork(S, 4))[10];

        /* Initialize work vectors */
        switch_status[0] = 0;
        switch_status_init[0] = 0;
        gState[0] = (int_T) 0.0;
        yswitch[0] = 1/0.001;
        switchTypes[0] = (int_T)7.0;
        idxOutSw[0] = ((int_T)0.0) - 1;
        switch_status[1] = 0;
        switch_status_init[1] = 0;
        gState[1] = (int_T) 0.0;
        yswitch[1] = 1/0.001;
        switchTypes[1] = (int_T)7.0;
        idxOutSw[1] = ((int_T)0.0) - 1;
        switch_status[2] = 0;
        switch_status_init[2] = 0;
        gState[2] = (int_T) 0.0;
        yswitch[2] = 1/0.001;
        switchTypes[2] = (int_T)7.0;
        idxOutSw[2] = ((int_T)0.0) - 1;
        switch_status[3] = 0;
        switch_status_init[3] = 0;
        gState[3] = (int_T) 0.0;
        yswitch[3] = 1/0.001;
        switchTypes[3] = (int_T)7.0;
        idxOutSw[3] = ((int_T)0.0) - 1;
        switch_status[4] = 0;
        switch_status_init[4] = 0;
        gState[4] = (int_T) 0.0;
        yswitch[4] = 1/0.001;
        switchTypes[4] = (int_T)7.0;
        idxOutSw[4] = ((int_T)0.0) - 1;
        switch_status[5] = 0;
        switch_status_init[5] = 0;
        gState[5] = (int_T) 0.0;
        yswitch[5] = 1/0.001;
        switchTypes[5] = (int_T)7.0;
        idxOutSw[5] = ((int_T)0.0) - 1;
      }
    }

    /* InitializeConditions for UnitDelay: '<S4>/Unit Delay' */
    ((real_T *)ssGetDWork(S, 3))[0] = 0.0;
  }
}

/* Start for root system: '<Root>' */
#define MDL_START

static void mdlStart(SimStruct *S)
{
  /* instance underlying S-Function data */
#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
#if defined(MATLAB_MEX_FILE)

  /* non-finites */
  rt_InitInfAndNaN(sizeof(real_T));

#endif

  PMSM_Motor_TL3_malloc(S);
  if (ssGetErrorStatus(S) != (NULL) ) {
    return;
  }

#endif

  {
    B_PMSM_Motor_TL3_T *_rtB;
    _rtB = ((B_PMSM_Motor_TL3_T *) ssGetLocalBlockIO(S));

    /* Start for S-Function (sfun_spssw_discc): '<S20>/State-Space' incorporates:
     *  Constant: '<S19>/Constant'
     */

    /* S-Function block: <S20>/State-Space */
    {
      ((void**) ssGetDWork(S, 4))[3] = (real_T*)calloc(9 * 9, sizeof(real_T));
      ((void**) ssGetDWork(S, 4))[4] = (real_T*)calloc(9, sizeof(real_T));
      ((void**) ssGetDWork(S, 4))[7] = (real_T*)calloc(9, sizeof(real_T));
      ((void**) ssGetDWork(S, 4))[9] = (int_T*)calloc(6, sizeof(int_T));
      ((void**) ssGetDWork(S, 4))[11] = (int_T*)calloc(6, sizeof(int_T));
      ((void**) ssGetDWork(S, 4))[12] = (int_T*)calloc(6, sizeof(int_T));
      ((void**) ssGetDWork(S, 4))[19] = (real_T*)calloc(6, sizeof(real_T));
      ((void**) ssGetDWork(S, 4))[20] = (int_T*)calloc(6, sizeof(int_T));
      ((void**) ssGetDWork(S, 4))[21] = (int_T*)calloc(6, sizeof(int_T));
      ((void**) ssGetDWork(S, 4))[10] = (int_T*)calloc(6, sizeof(int_T));
      ((void**) ssGetDWork(S, 4))[13] = (real_T*)calloc(6, sizeof(real_T));
    }
  }
}

/* Outputs for root system: '<Root>' */
static void mdlOutputs(SimStruct *S, int_T tid)
{
  B_PMSM_Motor_TL3_T *_rtB;
  real_T rtb_DataTypeConversion1;
  real_T rtb_DataTypeConversion2;
  real_T rtb_ElementaryMath_o1_tmp;
  real_T rtb_ElementaryMath_o2_tmp;
  _rtB = ((B_PMSM_Motor_TL3_T *) ssGetLocalBlockIO(S));

  /* Trigonometry: '<S13>/Elementary Math' incorporates:
   *  DiscreteIntegrator: '<S11>/Discrete-Time Integrator1'
   *  Trigonometry: '<S16>/Trigonometric Function'
   */
  rtb_ElementaryMath_o1_tmp = sin(((real_T *)ssGetDWork(S, 0))[0]);
  rtb_ElementaryMath_o2_tmp = cos(((real_T *)ssGetDWork(S, 0))[0]);

  /* Fcn: '<S15>/Fcn' incorporates:
   *  DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
   *  DiscreteIntegrator: '<S18>/Discrete-Time Integrator'
   *  Trigonometry: '<S13>/Elementary Math'
   */
  _rtB->Fcn = ((real_T *)ssGetDWork(S, 1))[0] * rtb_ElementaryMath_o2_tmp +
    ((real_T *)ssGetDWork(S, 2))[0] * rtb_ElementaryMath_o1_tmp;

  /* Fcn: '<S15>/Fcn1' incorporates:
   *  DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
   *  DiscreteIntegrator: '<S18>/Discrete-Time Integrator'
   *  Trigonometry: '<S13>/Elementary Math'
   */
  _rtB->Fcn1 = ((-((real_T *)ssGetDWork(S, 1))[0] - 1.7320508075688772 *
                 ((real_T *)ssGetDWork(S, 2))[0]) * rtb_ElementaryMath_o2_tmp +
                (1.7320508075688772 * ((real_T *)ssGetDWork(S, 1))[0] - ((real_T
    *)ssGetDWork(S, 2))[0]) * rtb_ElementaryMath_o1_tmp) * 0.5;

  /* S-Function (sfun_spssw_discc): '<S20>/State-Space' incorporates:
   *  Constant: '<S19>/Constant'
   */

  /* S-Function block: <S20>/State-Space */
  {
    real_T accum;

    /* Circuit has switches */
    int_T *switch_status = (int_T*) ((void**) ssGetDWork(S, 4))[9];
    int_T *switch_status_init = (int_T*) ((void**) ssGetDWork(S, 4))[10];
    int_T *SwitchChange = (int_T*) ((void**) ssGetDWork(S, 4))[11];
    int_T *gState = (int_T*) ((void**) ssGetDWork(S, 4))[12];
    real_T *yswitch = (real_T*)((void**) ssGetDWork(S, 4))[19];
    int_T *switchTypes = (int_T*) ((void**) ssGetDWork(S, 4))[20];
    int_T *idxOutSw = (int_T*) ((void**) ssGetDWork(S, 4))[21];
    real_T *DxCol = (real_T*)((void**) ssGetDWork(S, 4))[4];
    real_T *tmp2 = (real_T*)((void**) ssGetDWork(S, 4))[7];
    real_T *uswlast = (real_T*)((void**) ssGetDWork(S, 4))[13];
    int_T newState;
    int_T swChanged = 0;
    int loopsToDo = 20;
    real_T temp;

    /* keep an initial copy of switch_status*/
    memcpy(switch_status_init, switch_status, 6 * sizeof(int_T));
    memcpy(uswlast, &_rtB->StateSpace_o1[0], 6*sizeof(real_T));
    do {
      if (loopsToDo == 1) {            /* Need to reset some variables: */
        swChanged = 0;

        /* return to the original switch status*/
        {
          int_T i1;
          for (i1=0; i1 < 6; i1++) {
            swChanged = ((SwitchChange[i1] = switch_status_init[i1] -
                          switch_status[i1]) != 0) ? 1 : swChanged;
            switch_status[i1] = switch_status_init[i1];
          }
        }
      } else {
        /*
         * Compute outputs:
         * ---------------
         */
        real_T *Ds = (real_T*)((void**) ssGetDWork(S, 4))[3];

        {
          int_T i1;
          real_T *y0 = &_rtB->StateSpace_o1[0];
          for (i1=0; i1 < 9; i1++) {
            accum = 0.0;

            {
              int_T i2;
              const real_T *u0 = PMSM_Motor_TL3_ConstP.Constant_Value;
              for (i2=0; i2 < 6; i2++) {
                accum += *(Ds++) * u0[i2];
              }

              accum += *(Ds++) * _rtB->Fcn;
              accum += *(Ds++) * _rtB->Fcn1;
              accum += *(Ds++) * (*((const real_T**)ssGetInputPortSignalPtrs(S,
                2)))[0];
            }

            y0[i1] = accum;
          }
        }

        swChanged = 0;

        {
          int_T i1;
          real_T *y0 = &_rtB->StateSpace_o1[0];
          for (i1=0; i1 < 6; i1++) {
            newState = ((y0[i1] > 0.0) && (gState[i1] > 0)) || (y0[i1] < 0.0) ?
              1 : (((y0[i1] > 0.0) && gState[i1] == 0) ? 0 : switch_status[i1]);
            swChanged = ((SwitchChange[i1] = newState - switch_status[i1]) != 0)
              ? 1 : swChanged;
            switch_status[i1] = newState;/* Keep new state */
          }
        }
      }

      /*
       * Compute new As, Bs, Cs and Ds matrixes:
       * --------------------------------------
       */
      if (swChanged) {
        real_T *Ds = (real_T*)((void**) ssGetDWork(S, 4))[3];
        real_T a1;

        {
          int_T i1;
          for (i1=0; i1 < 6; i1++) {
            if (SwitchChange[i1] != 0) {
              a1 = 1000.0*SwitchChange[i1];
              temp = 1/(1-Ds[i1*10]*a1);

              {
                int_T i2;
                for (i2=0; i2 < 9; i2++) {
                  DxCol[i2]= Ds[i2 * 9 + i1]*temp*a1;
                }
              }

              DxCol[i1] = temp;

              /* Copy row nSw of Ds into tmp2 and zero it out in Ds */
              memcpy(tmp2, &Ds[i1 * 9], 9 * sizeof(real_T));
              memset(&Ds[i1 * 9], '\0', 9 * sizeof(real_T));

              /* Cs = Cs + DxCol * tmp1, Ds = Ds + DxCol * tmp2 *******************/
              {
                int_T i2;
                for (i2=0; i2 < 9; i2++) {
                  a1 = DxCol[i2];

                  {
                    int_T i3;
                    for (i3=0; i3 < 9; i3++) {
                      Ds[i2 * 9 + i3] += a1 * tmp2[i3];
                    }
                  }
                }
              }
            }
          }
        }
      }                                /* if (swChanged) */
    } while (swChanged > 0 && --loopsToDo > 0);

    if (loopsToDo == 0) {
      real_T *Ds = (real_T*)((void**) ssGetDWork(S, 4))[3];

      {
        int_T i1;
        real_T *y0 = &_rtB->StateSpace_o1[0];
        for (i1=0; i1 < 9; i1++) {
          accum = 0.0;

          {
            int_T i2;
            const real_T *u0 = PMSM_Motor_TL3_ConstP.Constant_Value;
            for (i2=0; i2 < 6; i2++) {
              accum += *(Ds++) * u0[i2];
            }

            accum += *(Ds++) * _rtB->Fcn;
            accum += *(Ds++) * _rtB->Fcn1;
            accum += *(Ds++) * (*((const real_T**)ssGetInputPortSignalPtrs(S, 2)))
              [0];
          }

          y0[i1] = accum;
        }
      }
    }

    /* Output new switches states */
    {
      int_T i1;
      real_T *y1 = &_rtB->StateSpace_o2[0];
      for (i1=0; i1 < 6; i1++) {
        y1[i1] = (real_T)switch_status[i1];
      }
    }
  }

  /* Fcn: '<S13>/Fcn3' incorporates:
   *  Fcn: '<S13>/Fcn2'
   *  Trigonometry: '<S13>/Elementary Math'
   */
  rtb_DataTypeConversion2 = 2.0 * _rtB->StateSpace_o1[6] + _rtB->StateSpace_o1[7];
  rtb_DataTypeConversion1 = (rtb_DataTypeConversion2 * rtb_ElementaryMath_o1_tmp
    + -1.7320508075688772 * _rtB->StateSpace_o1[7] * rtb_ElementaryMath_o2_tmp) *
    0.33333333333333331;

  /* Fcn: '<S13>/Fcn2' incorporates:
   *  Trigonometry: '<S13>/Elementary Math'
   */
  rtb_DataTypeConversion2 = (rtb_DataTypeConversion2 * rtb_ElementaryMath_o2_tmp
    + 1.7320508075688772 * _rtB->StateSpace_o1[7] * rtb_ElementaryMath_o1_tmp) *
    0.33333333333333331;

  /* Outport: '<Root>/Stator voltage Vs_q (V)' */
  ((real_T *)ssGetOutputPortSignal(S, 5))[0] = rtb_DataTypeConversion2;

  /* Gain: '<S11>/Gain' */
  _rtB->Gain = 4.0 * *((const real_T **)ssGetInputPortSignalPtrs(S, 1))[0];

  /* Sum: '<S18>/Sum1' incorporates:
   *  DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
   *  DiscreteIntegrator: '<S18>/Discrete-Time Integrator'
   *  Gain: '<S18>/1//Lq'
   *  Gain: '<S18>/R//Lq'
   *  Gain: '<S18>/lam//Lq'
   *  Product: '<S18>/Product1'
   */
  _rtB->Sum1 = ((5617.9775280898875 * rtb_DataTypeConversion2 -
                 61.797752808988761 * ((real_T *)ssGetDWork(S, 1))[0]) -
                ((real_T *)ssGetDWork(S, 2))[0] * _rtB->Gain) -
    67.415730337078656 * _rtB->Gain;

  /* Sum: '<S17>/Sum' incorporates:
   *  DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
   *  DiscreteIntegrator: '<S18>/Discrete-Time Integrator'
   *  Gain: '<S17>/1//Ld'
   *  Gain: '<S17>/R//Ld'
   *  Product: '<S17>/Product'
   */
  _rtB->Sum = (5617.9775280898875 * rtb_DataTypeConversion1 - 61.797752808988761
               * ((real_T *)ssGetDWork(S, 2))[0]) + _rtB->Gain * ((real_T *)
    ssGetDWork(S, 1))[0];

  /* Outport: '<Root>/Stator voltage Vs_d (V)' */
  ((real_T *)ssGetOutputPortSignal(S, 6))[0] = rtb_DataTypeConversion1;

  /* Sum: '<S4>/Add1' incorporates:
   *  Constant: '<S4>/Filter_Constant'
   *  Constant: '<S4>/One'
   *  Product: '<S4>/Product'
   *  Product: '<S4>/Product1'
   *  UnitDelay: '<S4>/Unit Delay'
   */
  _rtB->Add1 = _rtB->StateSpace_o1[8] * 0.001 + 0.999 * ((real_T *)ssGetDWork(S,
    3))[0];

  /* Outport: '<Root>/Stator current is_b (A)' */
  ((real_T *)ssGetOutputPortSignal(S, 1))[0] = _rtB->Fcn1;

  /* Outport: '<Root>/Stator current is_c (A)' incorporates:
   *  Sum: '<S15>/Sum'
   */
  ((real_T *)ssGetOutputPortSignal(S, 2))[0] = (0.0 - _rtB->Fcn1) - _rtB->Fcn;

  /* Outport: '<Root>/Stator current is_a (A)' */
  ((real_T *)ssGetOutputPortSignal(S, 0))[0] = _rtB->Fcn;

  /* Outport: '<Root>/Stator current is_d (A)' incorporates:
   *  DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
   */
  ((real_T *)ssGetOutputPortSignal(S, 4))[0] = ((real_T *)ssGetDWork(S, 2))[0];

  /* Outport: '<Root>/Electromagnetic torque Te (N*m)' incorporates:
   *  DiscreteIntegrator: '<S17>/Discrete-Time Integrator'
   *  DiscreteIntegrator: '<S18>/Discrete-Time Integrator'
   *  Fcn: '<S9>/Te '
   */
  ((real_T *)ssGetOutputPortSignal(S, 12))[0] = (0.0 * ((real_T *)ssGetDWork(S,
    1))[0] * ((real_T *)ssGetDWork(S, 2))[0] + 0.012 * ((real_T *)ssGetDWork(S,
    1))[0]) * 6.0;

  /* Outport: '<Root>/Stator current is_q (A)' incorporates:
   *  DiscreteIntegrator: '<S18>/Discrete-Time Integrator'
   */
  ((real_T *)ssGetOutputPortSignal(S, 3))[0] = ((real_T *)ssGetDWork(S, 1))[0];

  /* Outport: '<Root>/Rotor angle thetam (rad)' incorporates:
   *  DiscreteIntegrator: '<S11>/Discrete-Time Integrator1'
   *  Fcn: '<S11>/Fcn'
   */
  ((real_T *)ssGetOutputPortSignal(S, 11))[0] = (((real_T *)ssGetDWork(S, 0))[0]
    - 1.5707963267948966) / 4.0;

  /* Gain: '<S16>/rad2deg' incorporates:
   *  Trigonometry: '<S16>/Trigonometric Function2'
   */
  rtb_DataTypeConversion2 = 57.295779513082323 * PMSM_Motor_TL3_rt_atan2d_snf
    (rtb_ElementaryMath_o1_tmp, rtb_ElementaryMath_o2_tmp);

  /* Outport: '<Root>/Hall effect signal h_a' incorporates:
   *  Constant: '<S12>/Constant'
   *  Constant: '<S12>/Constant1'
   *  DataTypeConversion: '<S12>/Data Type Conversion'
   *  Logic: '<S12>/Logical Operator'
   *  RelationalOperator: '<S12>/Relational Operator1'
   *  RelationalOperator: '<S12>/Relational Operator2'
   */
  ((real_T *)ssGetOutputPortSignal(S, 7))[0] = ((rtb_DataTypeConversion2 >=
    -60.0) && (rtb_DataTypeConversion2 <= 120.0));

  /* Outport: '<Root>/Hall effect signal h_b' incorporates:
   *  Constant: '<S12>/Constant2'
   *  Constant: '<S12>/Constant3'
   *  DataTypeConversion: '<S12>/Data Type Conversion1'
   *  Logic: '<S12>/Logical Operator1'
   *  RelationalOperator: '<S12>/Relational Operator3'
   *  RelationalOperator: '<S12>/Relational Operator4'
   */
  ((real_T *)ssGetOutputPortSignal(S, 8))[0] = ((rtb_DataTypeConversion2 >= 60.0)
    || (rtb_DataTypeConversion2 <= -120.0));

  /* Outport: '<Root>/Hall effect signal h_c' incorporates:
   *  Constant: '<S12>/Constant4'
   *  Constant: '<S12>/Constant5'
   *  DataTypeConversion: '<S12>/Data Type Conversion2'
   *  Logic: '<S12>/Logical Operator2'
   *  RelationalOperator: '<S12>/Relational Operator5'
   *  RelationalOperator: '<S12>/Relational Operator6'
   */
  ((real_T *)ssGetOutputPortSignal(S, 9))[0] = ((rtb_DataTypeConversion2 >=
    -180.0) && (rtb_DataTypeConversion2 <= 0.0));

  /* Outport: '<Root>/Rotor speed wm (rad//s)' */
  ((real_T *)ssGetOutputPortSignal(S, 10))[0] = *((const real_T **)
    ssGetInputPortSignalPtrs(S, 1))[0];
  UNUSED_PARAMETER(tid);
}

/* Update for root system: '<Root>' */
#define MDL_UPDATE

static void mdlUpdate(SimStruct *S, int_T tid)
{
  B_PMSM_Motor_TL3_T *_rtB;
  _rtB = ((B_PMSM_Motor_TL3_T *) ssGetLocalBlockIO(S));

  /* Update for DiscreteIntegrator: '<S11>/Discrete-Time Integrator1' */
  ((real_T *)ssGetDWork(S, 0))[0] = 5.0E-7 * _rtB->Gain + ((real_T *)ssGetDWork
    (S, 0))[0];

  /* Update for DiscreteIntegrator: '<S18>/Discrete-Time Integrator' */
  ((real_T *)ssGetDWork(S, 1))[0] = 5.0E-7 * _rtB->Sum1 + ((real_T *)ssGetDWork
    (S, 1))[0];

  /* Update for DiscreteIntegrator: '<S17>/Discrete-Time Integrator' */
  ((real_T *)ssGetDWork(S, 2))[0] = 5.0E-7 * _rtB->Sum + ((real_T *)ssGetDWork(S,
    2))[0];

  /* Update for S-Function (sfun_spssw_discc): '<S20>/State-Space' incorporates:
   *  Constant: '<S19>/Constant'
   */
  {
    int_T *gState = (int_T*)((void**) ssGetDWork(S, 4))[12];

    /* Store switch gates values for next step */
    *(gState++) = (int_T) (*(const real_T **)ssGetInputPortSignalPtrs(S, 0))[0];
    *(gState++) = (int_T) (*(const real_T **)ssGetInputPortSignalPtrs(S, 0))[1];
    *(gState++) = (int_T) (*(const real_T **)ssGetInputPortSignalPtrs(S, 0))[2];
    *(gState++) = (int_T) (*(const real_T **)ssGetInputPortSignalPtrs(S, 0))[3];
    *(gState++) = (int_T) (*(const real_T **)ssGetInputPortSignalPtrs(S, 0))[4];
    *(gState++) = (int_T) (*(const real_T **)ssGetInputPortSignalPtrs(S, 0))[5];
  }

  /* Update for UnitDelay: '<S4>/Unit Delay' */
  ((real_T *)ssGetDWork(S, 3))[0] = _rtB->Add1;
  UNUSED_PARAMETER(tid);
}

/* Termination for root system: '<Root>' */
static void mdlTerminate(SimStruct *S)
{
  B_PMSM_Motor_TL3_T *_rtB;
  _rtB = ((B_PMSM_Motor_TL3_T *) ssGetLocalBlockIO(S));

  /* Terminate for S-Function (sfun_spssw_discc): '<S20>/State-Space' incorporates:
   *  Constant: '<S19>/Constant'
   */

  /* S-Function block: <S20>/State-Space */
  {
    /* Free memory */
    free(((void**) ssGetDWork(S, 4))[3]);
    free(((void**) ssGetDWork(S, 4))[4]);
    free(((void**) ssGetDWork(S, 4))[7]);

    /*
     * Circuit has switches*/
    free(((void**) ssGetDWork(S, 4))[12]);
    free(((void**) ssGetDWork(S, 4))[9]);
    free(((void**) ssGetDWork(S, 4))[11]);
    free(((void**) ssGetDWork(S, 4))[10]);
  }

#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)

  if (ssGetUserData(S) != (NULL) ) {
    rt_FREE(ssGetLocalBlockIO(S));
  }

  rt_FREE(ssGetUserData(S));

#endif

}

#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
#include "PMSM_Motor_TL3_mid.h"
#endif

/* Function to initialize sizes. */
static void mdlInitializeSizes(SimStruct *S)
{
  ssSetNumSampleTimes(S, 1);           /* Number of sample times */
  ssSetNumContStates(S, 0);            /* Number of continuous states */
  ssSetNumNonsampledZCs(S, 0);         /* Number of nonsampled ZCs */

  /* Number of output ports */
  if (!ssSetNumOutputPorts(S, 13))
    return;

  /* outport number: 0 */
  if (!ssSetOutputPortVectorDimension(S, 0, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 0, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 0, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 0, 0.0);
  ssSetOutputPortOptimOpts(S, 0, SS_REUSABLE_AND_LOCAL);

  /* outport number: 1 */
  if (!ssSetOutputPortVectorDimension(S, 1, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 1, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 1, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 1, 0.0);
  ssSetOutputPortOptimOpts(S, 1, SS_REUSABLE_AND_LOCAL);

  /* outport number: 2 */
  if (!ssSetOutputPortVectorDimension(S, 2, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 2, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 2, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 2, 0.0);
  ssSetOutputPortOptimOpts(S, 2, SS_REUSABLE_AND_LOCAL);

  /* outport number: 3 */
  if (!ssSetOutputPortVectorDimension(S, 3, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 3, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 3, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 3, 0.0);
  ssSetOutputPortOptimOpts(S, 3, SS_REUSABLE_AND_LOCAL);

  /* outport number: 4 */
  if (!ssSetOutputPortVectorDimension(S, 4, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 4, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 4, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 4, 0.0);
  ssSetOutputPortOptimOpts(S, 4, SS_REUSABLE_AND_LOCAL);

  /* outport number: 5 */
  if (!ssSetOutputPortVectorDimension(S, 5, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 5, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 5, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 5, 0.0);
  ssSetOutputPortOptimOpts(S, 5, SS_REUSABLE_AND_LOCAL);

  /* outport number: 6 */
  if (!ssSetOutputPortVectorDimension(S, 6, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 6, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 6, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 6, 0.0);
  ssSetOutputPortOptimOpts(S, 6, SS_REUSABLE_AND_LOCAL);

  /* outport number: 7 */
  if (!ssSetOutputPortVectorDimension(S, 7, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 7, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 7, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 7, 0.0);
  ssSetOutputPortOptimOpts(S, 7, SS_REUSABLE_AND_LOCAL);

  /* outport number: 8 */
  if (!ssSetOutputPortVectorDimension(S, 8, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 8, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 8, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 8, 0.0);
  ssSetOutputPortOptimOpts(S, 8, SS_REUSABLE_AND_LOCAL);

  /* outport number: 9 */
  if (!ssSetOutputPortVectorDimension(S, 9, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 9, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 9, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 9, 0.0);
  ssSetOutputPortOptimOpts(S, 9, SS_REUSABLE_AND_LOCAL);

  /* outport number: 10 */
  if (!ssSetOutputPortVectorDimension(S, 10, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 10, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 10, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 10, 0.0);
  ssSetOutputPortOptimOpts(S, 10, SS_REUSABLE_AND_LOCAL);

  /* outport number: 11 */
  if (!ssSetOutputPortVectorDimension(S, 11, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 11, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 11, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 11, 0.0);
  ssSetOutputPortOptimOpts(S, 11, SS_REUSABLE_AND_LOCAL);

  /* outport number: 12 */
  if (!ssSetOutputPortVectorDimension(S, 12, 1))
    return;
  if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
    ssSetOutputPortDataType(S, 12, SS_DOUBLE);
  }

  ssSetOutputPortSampleTime(S, 12, 5.0E-7);
  ssSetOutputPortOffsetTime(S, 12, 0.0);
  ssSetOutputPortOptimOpts(S, 12, SS_REUSABLE_AND_LOCAL);

  /* Number of input ports */
  if (!ssSetNumInputPorts(S, 3))
    return;

  /* inport number: 0 */
  {
    if (!ssSetInputPortVectorDimension(S, 0, 6))
      return;
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
      ssSetInputPortDataType(S, 0, SS_DOUBLE);
    }

    ssSetInputPortSampleTime(S, 0, 5.0E-7);
    ssSetInputPortOffsetTime(S, 0, 0.0);
    ssSetInputPortOverWritable(S, 0, 0);
    ssSetInputPortOptimOpts(S, 0, SS_NOT_REUSABLE_AND_GLOBAL);
  }

  /* inport number: 1 */
  {
    if (!ssSetInputPortVectorDimension(S, 1, 1))
      return;
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
      ssSetInputPortDataType(S, 1, SS_DOUBLE);
    }

    ssSetInputPortDirectFeedThrough(S, 1, 1);
    ssSetInputPortSampleTime(S, 1, 5.0E-7);
    ssSetInputPortOffsetTime(S, 1, 0.0);
    ssSetInputPortOverWritable(S, 1, 0);
    ssSetInputPortOptimOpts(S, 1, SS_NOT_REUSABLE_AND_GLOBAL);
  }

  /* inport number: 2 */
  {
    if (!ssSetInputPortVectorDimension(S, 2, 1))
      return;
    if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
      ssSetInputPortDataType(S, 2, SS_DOUBLE);
    }

    ssSetInputPortDirectFeedThrough(S, 2, 1);
    ssSetInputPortSampleTime(S, 2, 5.0E-7);
    ssSetInputPortOffsetTime(S, 2, 0.0);
    ssSetInputPortOverWritable(S, 2, 0);
    ssSetInputPortOptimOpts(S, 2, SS_NOT_REUSABLE_AND_GLOBAL);
  }

  ssSetRTWGeneratedSFcn(S, 1);         /* Generated S-function */

  /* DWork */
  if (!ssSetNumDWork(S, 6)) {
    return;
  }

  /* '<S11>/Discrete-Time Integrator1': DSTATE */
  ssSetDWorkName(S, 0, "DWORK0");
  ssSetDWorkWidth(S, 0, 1);
  ssSetDWorkUsedAsDState(S, 0, 1);

  /* '<S18>/Discrete-Time Integrator': DSTATE */
  ssSetDWorkName(S, 1, "DWORK1");
  ssSetDWorkWidth(S, 1, 1);
  ssSetDWorkUsedAsDState(S, 1, 1);

  /* '<S17>/Discrete-Time Integrator': DSTATE */
  ssSetDWorkName(S, 2, "DWORK2");
  ssSetDWorkWidth(S, 2, 1);
  ssSetDWorkUsedAsDState(S, 2, 1);

  /* '<S4>/Unit Delay': DSTATE */
  ssSetDWorkName(S, 3, "DWORK3");
  ssSetDWorkWidth(S, 3, 1);
  ssSetDWorkUsedAsDState(S, 3, 1);

  /* '<S20>/State-Space': PWORK */
  ssSetDWorkName(S, 4, "DWORK4");
  ssSetDWorkWidth(S, 4, 24);
  ssSetDWorkDataType(S, 4, SS_POINTER);

  /* '<S20>/State-Space': IWORK */
  ssSetDWorkName(S, 5, "DWORK5");
  ssSetDWorkWidth(S, 5, 11);
  ssSetDWorkDataType(S, 5, SS_INTEGER);

  /* Tunable Parameters */
  ssSetNumSFcnParams(S, 0);

  /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)

  if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {

#if defined(MDL_CHECK_PARAMETERS)

    mdlCheckParameters(S);

#endif                                 /* MDL_CHECK_PARAMETERS */

    if (ssGetErrorStatus(S) != (NULL) ) {
      return;
    }
  } else {
    return;                /* Parameter mismatch will be reported by Simulink */
  }

#endif                                 /* MATLAB_MEX_FILE */

  /* Options */
  ssSetOptions(S, (SS_OPTION_RUNTIME_EXCEPTION_FREE_CODE |
                   SS_OPTION_PORT_SAMPLE_TIMES_ASSIGNED ));

#if SS_SFCN_FOR_SIM

  {
    ssSupportsMultipleExecInstances(S, true);
    ssHasStateInsideForEachSS(S, false);
  }

#endif

}

/* Function to initialize sample times. */
static void mdlInitializeSampleTimes(SimStruct *S)
{
  /* task periods */
  ssSetSampleTime(S, 0, 5.0E-7);

  /* task offsets */
  ssSetOffsetTime(S, 0, 0.0);
}

#if defined(MATLAB_MEX_FILE)
#include "fixedpoint.c"
#include "simulink.c"
#else
#undef S_FUNCTION_NAME
#define S_FUNCTION_NAME                PMSM_Motor_TL3_sf
#include "cg_sfun.h"
#endif                                 /* defined(MATLAB_MEX_FILE) */
