/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * File: Controller.c
 *
 * Code generated for Simulink model 'Controller'.
 *
 * Model version                  : 2.23
 * Simulink Coder version         : 25.1 (R2025a) 21-Nov-2024
 * C/C++ source code generated on : Sat Jun 14 13:33:39 2025
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: ARM Compatible->ARM Cortex-M
 * Code generation objectives:
 *    1. Execution efficiency
 *    2. RAM efficiency
 * Validation result: Not run
 */

#include "Controller.h"
#include "rtwtypes.h"
#include <math.h>
#include "arm_math.h"

/* Block signals and states (default storage) */
DW rtDW;

/* External inputs (root inport signals with default storage) */
ExtU rtU;

/* External outputs (root outports fed by signals with default storage) */
ExtY rtY;

/* Real-time model */
static RT_MODEL rtM_;
RT_MODEL *const rtM = &rtM_;
static real32_T IfActionSubsystem(real32_T rtu_In1);
static real32_T IfActionSubsystem1(real32_T rtu_In1);

/*
 * Output and update for action system:
 *    '<S22>/If Action Subsystem'
 *    '<S193>/If Action Subsystem'
 */
static real32_T IfActionSubsystem(real32_T rtu_In1)
{
  /* Sum: '<S25>/Sum' incorporates:
   *  DataTypeConversion: '<S25>/Convert_back'
   *  DataTypeConversion: '<S25>/Convert_uint16'
   */
  return rtu_In1 - (real32_T)(int16_T)floorf(rtu_In1);
}

/*
 * Output and update for action system:
 *    '<S22>/If Action Subsystem1'
 *    '<S193>/If Action Subsystem1'
 */
static real32_T IfActionSubsystem1(real32_T rtu_In1)
{
  /* Sum: '<S26>/Sum' incorporates:
   *  DataTypeConversion: '<S26>/Convert_back'
   *  DataTypeConversion: '<S26>/Convert_uint16'
   */
  return rtu_In1 - (real32_T)(int16_T)rtu_In1;
}

/* Model step function */
void Controller_step(void)
{
  real32_T T_idx_0;
  real32_T T_idx_1;
  real32_T T_idx_2;
  real32_T T_idx_3;
  real32_T d_Temp;
  real32_T rtb_Gain1;
  real32_T rtb_Sum2;
  real32_T rtb_Sum4_f;
  real32_T rtb_algDD_o1;
  real32_T rtb_algDD_o1_n;
  real32_T rtb_algDD_o2_h;
  real32_T rtb_convert_pu;
  real32_T rtb_sum_alpha;
  real32_T u0;
  real32_T u0_0;
  uint16_T rtb_Get_Integer_e;

  /* Gain: '<S22>/convert_pu' incorporates:
   *  Gain: '<S1>/Gain'
   *  Inport: '<Root>/theta'
   */
  rtb_convert_pu = 7.0F * rtU.Rotoranglethetamrad * 0.159154937F;

  /* If: '<S22>/If' incorporates:
   *  Constant: '<S24>/Constant'
   *  RelationalOperator: '<S24>/Compare'
   */
  if (rtb_convert_pu < 0.0F) {
    /* Outputs for IfAction SubSystem: '<S22>/If Action Subsystem' incorporates:
     *  ActionPort: '<S25>/Action Port'
     */
    T_idx_2 = IfActionSubsystem(rtb_convert_pu);

    /* End of Outputs for SubSystem: '<S22>/If Action Subsystem' */
  } else {
    /* Outputs for IfAction SubSystem: '<S22>/If Action Subsystem1' incorporates:
     *  ActionPort: '<S26>/Action Port'
     */
    T_idx_2 = IfActionSubsystem1(rtb_convert_pu);

    /* End of Outputs for SubSystem: '<S22>/If Action Subsystem1' */
  }

  /* End of If: '<S22>/If' */

  /* Gain: '<S20>/indexing' */
  T_idx_2 *= 800.0F;

  /* If: '<S193>/If' incorporates:
   *  Constant: '<S195>/Constant'
   *  RelationalOperator: '<S195>/Compare'
   */
  if (rtb_convert_pu < 0.0F) {
    /* Outputs for IfAction SubSystem: '<S193>/If Action Subsystem' incorporates:
     *  ActionPort: '<S196>/Action Port'
     */
    rtb_Sum2 = IfActionSubsystem(rtb_convert_pu);

    /* End of Outputs for SubSystem: '<S193>/If Action Subsystem' */
  } else {
    /* Outputs for IfAction SubSystem: '<S193>/If Action Subsystem1' incorporates:
     *  ActionPort: '<S197>/Action Port'
     */
    rtb_Sum2 = IfActionSubsystem1(rtb_convert_pu);

    /* End of Outputs for SubSystem: '<S193>/If Action Subsystem1' */
  }

  /* End of If: '<S193>/If' */

  /* Gain: '<S191>/indexing' */
  rtb_Sum2 *= 800.0F;

  /* DataTypeConversion: '<S191>/Get_Integer' */
  rtb_Get_Integer_e = (uint16_T)rtb_Sum2;

  /* Sum: '<S191>/Sum2' incorporates:
   *  DataTypeConversion: '<S191>/Data Type Conversion1'
   *  DataTypeConversion: '<S191>/Get_Integer'
   */
  rtb_Sum2 -= (real32_T)(uint16_T)rtb_Sum2;

  /* Outputs for Atomic SubSystem: '<S13>/Two phase CRL wrap' */
  /* AlgorithmDescriptorDelegate generated from: '<S14>/a16' incorporates:
   *  Inport: '<Root>/Ia'
   *  Inport: '<Root>/Ib'
   */
  arm_clarke_f32(rtU.Ia, rtU.Ib, &rtb_convert_pu, &rtb_algDD_o2_h);

  /* End of Outputs for SubSystem: '<S13>/Two phase CRL wrap' */

  /* Outputs for Atomic SubSystem: '<S187>/Two inputs CRL' */
  /* AlgorithmDescriptorDelegate generated from: '<S189>/a16' incorporates:
   *  Constant: '<S191>/offset'
   *  Constant: '<S191>/sine_table_values'
   *  Gain: '<S13>/Kalpha'
   *  Gain: '<S13>/Kbeta'
   *  Product: '<S192>/Product'
   *  Product: '<S192>/Product1'
   *  Selector: '<S191>/Lookup'
   *  Sum: '<S191>/Sum'
   *  Sum: '<S192>/Sum3'
   *  Sum: '<S192>/Sum4'
   *  Sum: '<S192>/Sum5'
   *  Sum: '<S192>/Sum6'
   */
  arm_park_f32(1.22474492F * rtb_convert_pu, 1.22474492F * rtb_algDD_o2_h,
               &rtb_algDD_o1, &T_idx_0, (rtConstP.pooled1[(int32_T)
    (rtb_Get_Integer_e + 1U)] - rtConstP.pooled1[rtb_Get_Integer_e]) * rtb_Sum2
               + rtConstP.pooled1[rtb_Get_Integer_e], (rtConstP.pooled1[(int32_T)
    (rtb_Get_Integer_e + 201U)] - rtConstP.pooled1[(int32_T)(rtb_Get_Integer_e +
    200U)]) * rtb_Sum2 + rtConstP.pooled1[(int32_T)(rtb_Get_Integer_e + 200U)]);

  /* End of Outputs for SubSystem: '<S187>/Two inputs CRL' */

  /* SampleTimeMath: '<S3>/TSamp' incorporates:
   *  Inport: '<Root>/theta'
   *
   * About '<S3>/TSamp':
   *  y = u * K where K = 1 / ( w * Ts )
   *   */
  rtb_convert_pu = rtU.Rotoranglethetamrad * 10000.0F;

  /* Sum: '<S3>/Diff' incorporates:
   *  UnitDelay: '<S3>/UD'
   *
   * Block description for '<S3>/Diff':
   *
   *  Add in CPU
   *
   * Block description for '<S3>/UD':
   *
   *  Store in Global RAM
   */
  rtb_Sum2 = rtb_convert_pu - rtDW.UD_DSTATE;

  /* Gain: '<S1>/Gain1' */
  rtb_Gain1 = 7.0F * rtb_Sum2;

  /* Sum: '<S1>/Sum3' incorporates:
   *  Inport: '<Root>/Id_ref'
   */
  rtb_algDD_o2_h = rtU.Id_ref - rtb_algDD_o1;

  /* Sum: '<S1>/Sum4' incorporates:
   *  Gain: '<S1>/Gain2'
   *  Inport: '<Root>/Speed_ref'
   */
  rtb_Sum4_f = rtU.Speed_ref - 9.54929638F * rtb_Sum2;

  /* Sum: '<S1>/Sum2' incorporates:
   *  DiscreteIntegrator: '<S167>/Integrator'
   *  Gain: '<S172>/Proportional Gain'
   *  Sum: '<S176>/Sum'
   */
  rtb_Sum2 = (0.026F * rtb_Sum4_f + rtDW.Integrator_DSTATE_n) - T_idx_0;

  /* Sum: '<S20>/Sum2' incorporates:
   *  DataTypeConversion: '<S20>/Data Type Conversion1'
   *  DataTypeConversion: '<S20>/Get_Integer'
   */
  rtb_sum_alpha = T_idx_2 - (real32_T)(uint16_T)T_idx_2;

  /* Sum: '<S124>/Sum' incorporates:
   *  DiscreteIntegrator: '<S115>/Integrator'
   *  Gain: '<S120>/Proportional Gain'
   */
  u0 = 0.558F * rtb_algDD_o2_h + rtDW.Integrator_DSTATE;

  /* Sum: '<S72>/Sum' incorporates:
   *  DiscreteIntegrator: '<S63>/Integrator'
   *  Gain: '<S68>/Proportional Gain'
   */
  u0_0 = 0.558F * rtb_Sum2 + rtDW.Integrator_DSTATE_h;

  /* Saturate: '<S122>/Saturation' */
  if (u0 > 6.92820311F) {
    u0 = 6.92820311F;
  } else if (u0 < -6.92820311F) {
    u0 = -6.92820311F;
  }

  /* Saturate: '<S70>/Saturation' */
  if (u0_0 > 6.92820311F) {
    u0_0 = 6.92820311F;
  } else if (u0_0 < -6.92820311F) {
    u0_0 = -6.92820311F;
  }

  /* Outputs for Atomic SubSystem: '<S17>/Two inputs CRL' */
  /* AlgorithmDescriptorDelegate generated from: '<S19>/a16' incorporates:
   *  Constant: '<S20>/offset'
   *  Constant: '<S20>/sine_table_values'
   *  DataTypeConversion: '<S20>/Get_Integer'
   *  Fcn: '<S1>/Fcn'
   *  Fcn: '<S1>/Fcn1'
   *  Product: '<S21>/Product'
   *  Product: '<S21>/Product1'
   *  Saturate: '<S122>/Saturation'
   *  Saturate: '<S70>/Saturation'
   *  Selector: '<S20>/Lookup'
   *  Sum: '<S1>/Sum'
   *  Sum: '<S1>/Sum1'
   *  Sum: '<S20>/Sum'
   *  Sum: '<S21>/Sum3'
   *  Sum: '<S21>/Sum4'
   *  Sum: '<S21>/Sum5'
   *  Sum: '<S21>/Sum6'
   */
  arm_inv_park_f32(u0 + -rtb_Gain1 * T_idx_0 * 0.000279F, u0_0 + rtb_Gain1 *
                   (0.000279F * rtb_algDD_o1 + 0.005753F), &rtb_algDD_o1_n,
                   &T_idx_2, (rtConstP.pooled1[(int32_T)((uint16_T)T_idx_2 + 1U)]
    - rtConstP.pooled1[(uint16_T)T_idx_2]) * rtb_sum_alpha + rtConstP.pooled1
                   [(uint16_T)T_idx_2], (rtConstP.pooled1[(int32_T)((uint16_T)
    T_idx_2 + 201U)] - rtConstP.pooled1[(int32_T)((uint16_T)T_idx_2 + 200U)]) *
                   rtb_sum_alpha + rtConstP.pooled1[(int32_T)((uint16_T)T_idx_2
    + 200U)]);

  /* End of Outputs for SubSystem: '<S17>/Two inputs CRL' */

  /* MATLAB Function: '<S9>/MATLAB Function2' */
  switch ((((T_idx_2 - rtb_algDD_o1_n * 1.73205078F > 0.0F) << 2) +
           ((rtb_algDD_o1_n * 1.73205078F + T_idx_2 > 0.0F) << 1)) + (T_idx_2 <=
           0.0F)) {
   case 1:
    T_idx_0 = 5.0F;
    break;

   case 2:
    T_idx_0 = 1.0F;
    break;

   case 3:
    T_idx_0 = 6.0F;
    break;

   case 4:
    T_idx_0 = 3.0F;
    break;

   case 5:
    T_idx_0 = 4.0F;
    break;

   case 6:
    T_idx_0 = 2.0F;
    break;

   default:
    T_idx_0 = -1.0F;
    break;
  }

  /* End of MATLAB Function: '<S9>/MATLAB Function2' */

  /* MATLAB Function: '<S9>/MATLAB Function1' incorporates:
   *  Constant: '<S9>/Constant'
   *  Constant: '<S9>/Constant1'
   */
  switch ((int32_T)T_idx_0) {
   case 1:
    rtb_algDD_o1 = 2.0F * T_idx_2 * 1800.0F / 1.73205078F / 8.0F;
    T_idx_3 = (rtb_algDD_o1_n - T_idx_2 / 1.73205078F) * 1800.0F / 8.0F;
    T_idx_1 = rtb_algDD_o1;
    d_Temp = T_idx_3 + rtb_algDD_o1;
    if (d_Temp > 1800.0F) {
      d_Temp = 1800.0F / d_Temp;
      T_idx_3 *= d_Temp;
      T_idx_1 = rtb_algDD_o1 * d_Temp;
    }

    d_Temp = ((1800.0F - T_idx_3) - T_idx_1) / 2.0F;
    T_idx_3 = (T_idx_3 + T_idx_1) + d_Temp;
    T_idx_1 += d_Temp;
    break;

   case 2:
    T_idx_3 = T_idx_2 / 1.73205078F;
    T_idx_1 = (T_idx_3 - rtb_algDD_o1_n) * 1800.0F / 8.0F;
    rtb_algDD_o1 = (T_idx_3 + rtb_algDD_o1_n) * 1800.0F / 8.0F;
    T_idx_2 = rtb_algDD_o1;
    d_Temp = T_idx_1 + rtb_algDD_o1;
    if (d_Temp > 1800.0F) {
      d_Temp = 1800.0F / d_Temp;
      T_idx_1 *= d_Temp;
      T_idx_2 = rtb_algDD_o1 * d_Temp;
    }

    d_Temp = ((1800.0F - T_idx_1) - T_idx_2) / 2.0F;
    T_idx_3 = T_idx_2 + d_Temp;
    T_idx_1 = (T_idx_1 + T_idx_2) + d_Temp;
    break;

   case 3:
    T_idx_1 = 1.73205078F * T_idx_2 * 1800.0F / 12.0F;
    T_idx_2 = -((1.5F * rtb_algDD_o1_n + 0.866025388F * T_idx_2) * 150.0F);
    d_Temp = T_idx_1 + T_idx_2;
    if (d_Temp > 1800.0F) {
      d_Temp = 1800.0F / d_Temp;
      T_idx_1 *= d_Temp;
      T_idx_2 *= d_Temp;
    }

    d_Temp = ((1800.0F - T_idx_1) - T_idx_2) / 2.0F;
    T_idx_3 = d_Temp;
    T_idx_1 = (T_idx_1 + T_idx_2) + d_Temp;
    d_Temp += T_idx_2;
    break;

   case 4:
    T_idx_0 = -2.0F * T_idx_2 * 1800.0F / 1.73205078F / 8.0F;
    T_idx_2 = (T_idx_2 / 1.73205078F - rtb_algDD_o1_n) * 1800.0F / 8.0F;
    d_Temp = T_idx_0 + T_idx_2;
    if (d_Temp > 1800.0F) {
      d_Temp = 1800.0F / d_Temp;
      T_idx_0 *= d_Temp;
      T_idx_2 *= d_Temp;
    }

    d_Temp = ((1800.0F - T_idx_0) - T_idx_2) / 2.0F;
    T_idx_3 = d_Temp;
    T_idx_1 = T_idx_2 + d_Temp;
    d_Temp += T_idx_0 + T_idx_2;
    break;

   case 5:
    T_idx_3 = -T_idx_2 / 1.73205078F;
    T_idx_0 = (T_idx_3 - rtb_algDD_o1_n) * 1800.0F / 8.0F;
    T_idx_1 = (T_idx_3 + rtb_algDD_o1_n) * 1800.0F / 8.0F;
    T_idx_2 = T_idx_1;
    d_Temp = T_idx_0 + T_idx_1;
    if (d_Temp > 1800.0F) {
      d_Temp = 1800.0F / d_Temp;
      T_idx_0 *= d_Temp;
      T_idx_2 = T_idx_1 * d_Temp;
    }

    d_Temp = ((1800.0F - T_idx_0) - T_idx_2) / 2.0F;
    T_idx_3 = T_idx_2 + d_Temp;
    T_idx_1 = d_Temp;
    d_Temp += T_idx_0 + T_idx_2;
    break;

   case 6:
    T_idx_1 = -2.0F * T_idx_2 * 1800.0F / 1.73205078F / 8.0F;
    T_idx_3 = (T_idx_2 / 1.73205078F + rtb_algDD_o1_n) * 1800.0F / 8.0F;
    T_idx_2 = T_idx_1;
    d_Temp = T_idx_3 + T_idx_1;
    if (d_Temp > 1800.0F) {
      d_Temp = 1800.0F / d_Temp;
      T_idx_3 *= d_Temp;
      T_idx_2 = T_idx_1 * d_Temp;
    }

    d_Temp = ((1800.0F - T_idx_3) - T_idx_2) / 2.0F;
    T_idx_3 = (T_idx_3 + T_idx_2) + d_Temp;
    T_idx_1 = d_Temp;
    d_Temp += T_idx_2;
    break;
  }

  /* Outport: '<Root>/CCR1' incorporates:
   *  Constant: '<S9>/Constant1'
   *  MATLAB Function: '<S9>/MATLAB Function1'
   */
  rtY.CCR1 = (1800.0F - T_idx_3) / 2.0F;

  /* Outport: '<Root>/CCR2' incorporates:
   *  Constant: '<S9>/Constant1'
   *  MATLAB Function: '<S9>/MATLAB Function1'
   */
  rtY.CCR2 = (1800.0F - T_idx_1) / 2.0F;

  /* Outport: '<Root>/CCR3' incorporates:
   *  Constant: '<S9>/Constant1'
   *  MATLAB Function: '<S9>/MATLAB Function1'
   */
  rtY.CCR3 = (1800.0F - d_Temp) / 2.0F;

  /* Update for UnitDelay: '<S3>/UD'
   *
   * Block description for '<S3>/UD':
   *
   *  Store in Global RAM
   */
  rtDW.UD_DSTATE = rtb_convert_pu;

  /* Update for DiscreteIntegrator: '<S115>/Integrator' incorporates:
   *  Gain: '<S112>/Integral Gain'
   */
  rtDW.Integrator_DSTATE += 1600.0F * rtb_algDD_o2_h * 0.0001F;

  /* Update for DiscreteIntegrator: '<S167>/Integrator' incorporates:
   *  Gain: '<S164>/Integral Gain'
   */
  rtDW.Integrator_DSTATE_n += 2.408F * rtb_Sum4_f * 0.0001F;

  /* Update for DiscreteIntegrator: '<S63>/Integrator' incorporates:
   *  Gain: '<S60>/Integral Gain'
   */
  rtDW.Integrator_DSTATE_h += 1600.0F * rtb_Sum2 * 0.0001F;
}

/* Model initialize function */
void Controller_initialize(void)
{
  /* (no initialization code required) */
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */