#include "SOC.h"
#include "rtwtypes.h"
#include <math.h>
#include "rt_nonfinite.h"
#include "SOC_private.h"
#include "BCUDisp.h"
#include "zero_crossing_types.h"
#include "look1_iflf_binlcapw.h"
#include "BCUCal.h"

MdlrefDW_SOC_T SOC_MdlrefDW;
B_SOC_c_T SOC_B;
DW_SOC_f_T SOC_DW;
ZCE_SOC_T SOC_PrevZCX;
void SOC_IfActionSubsystem(real32_T rtu_Fitdata, real32_T *rty_Out1, real32_T
  rtp_m)
{
  *rty_Out1 = rtp_m + rtu_Fitdata;
}

void SOC_IfActionSubsystem1(real32_T rtu_Fitdata1, real32_T *rty_Out2, real32_T
  rtp_m)
{
  *rty_Out2 = -rtp_m + rtu_Fitdata1;
}

void SOC_Subsystem1(boolean_T rtu_Trigger, real32_T rtu_in1, real32_T rtu_in2,
                    real32_T *rty_out1, real32_T *rty_out2, ZCE_Subsystem1_SOC_T
                    *localZCE)
{
  if (rtu_Trigger && (localZCE->Subsystem1_Trig_ZCE != POS_ZCSIG)) {
    *rty_out1 = rtu_in1;
    *rty_out2 = rtu_in2;
  }

  localZCE->Subsystem1_Trig_ZCE = rtu_Trigger;
}

void SOC_Init(void)
{
  SOC_DW.icLoad = true;
  SOC_DW.icLoad_j = true;
  SOC_DW.icLoad_o = true;
  SOC_DW.icLoad_i = true;
  SOC_DW.icLoad_p = true;
  SOC_DW.P_Delay_DSTATE[0] = 0.001F;
  SOC_DW.P_Delay_DSTATE_k[0] = 0.001F;
  SOC_DW.P_Delay_DSTATE[1] = 0.0F;
  SOC_DW.P_Delay_DSTATE_k[1] = 0.0F;
  SOC_DW.P_Delay_DSTATE[2] = 0.0F;
  SOC_DW.P_Delay_DSTATE_k[2] = 0.0F;
  SOC_DW.P_Delay_DSTATE[3] = 0.001F;
  SOC_DW.P_Delay_DSTATE_k[3] = 0.001F;
  SOC_DW.icLoad_a = true;
  SOC_DW.icLoad_l = true;
  SOC_DW.icLoad_m = true;
  SOC_DW.socd_pct_battSoc0_Delay_DSTATE = 1.0F;
  SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE = 1.0F;
  SOC_DW.icLoad_g = true;
  SOC_DW.icLoad_n = true;
}

void SOC_Disable(void)
{
  SOC_DW.If_ActiveSubsystem = -1;
  SOC_DW.If_ActiveSubsystem_j = -1;
}

void SOC_Start(void)
{
  SOC_DW.If_ActiveSubsystem = -1;
  SOC_DW.If_ActiveSubsystem_j = -1;
}

void SOC(void)
{
  real_T rtb_Switch_n;
  real_T tmp_3;
  int_T idxDelay;
  real32_T rtb_Add_e[4];
  real32_T rtb_Add_mg[4];
  real32_T rtb_MatrixConcatenate[4];
  real32_T tmp[4];
  real32_T rtb_Divide_f[2];
  real32_T rtb_MathFunction1_b[2];
  real32_T rtb_MathFunction1_l[2];
  real32_T rtb_Abs1;
  real32_T rtb_Add6;
  real32_T rtb_Add_fu_tmp_tmp;
  real32_T rtb_Add_ht_tmp_tmp;
  real32_T rtb_Divide_l;
  real32_T rtb_Gain;
  real32_T rtb_MathFunction1_g;
  real32_T rtb_Merge3;
  real32_T rtb_OCVSOC1;
  real32_T tmp_0;
  real32_T tmp_1;
  real32_T tmp_2;
  uint16_T rtb_Gain3;
  uint16_T rtb_Gain3_l;
  int8_T rtPrevAction;
  boolean_T rtb_LogicalOperator3_i;
  if (SOC_DW.UnitDelay_DSTATE + 1.0 <= SOC_ConstB.Add) {
    SOC_DW.UnitDelay_DSTATE++;
  } else {
    SOC_DW.UnitDelay_DSTATE = SOC_ConstB.Add;
  }

  if (SOC_DW.UnitDelay_DSTATE == 1.0) {
    rtb_OCVSOC1 = look1_iflf_binlcapw(ihd_V_cellUAvrg, &cmnm_V_ocv[0],
      &cmnm_pct_soc[0], 12U);
    if (ihv_flg_EESt[1] || (socd_pct_battSocEi > 100.0F) || (socd_pct_bcuSocEi >
         100.0F)) {
      SOC_B.Switch1 = rtb_OCVSOC1;
      SOC_B.Switch = rtb_OCVSOC1;
    } else {
      SOC_B.Switch1 = socd_pct_battSocEi;
      SOC_B.Switch = socd_pct_bcuSocEi;
    }

    rtb_LogicalOperator3_i = ((ihd_tm_parkTime >= cmnc_tm_parkTime) &&
      ((rtb_OCVSOC1 >= 95.0F) || (rtb_OCVSOC1 <= 30.0F)));
    if (rtb_LogicalOperator3_i) {
      SOC_B.Switch1 = rtb_OCVSOC1;
    }

    if ((fabsf(SOC_B.Switch1 - SOC_B.Switch) > 50.0F) && rtb_LogicalOperator3_i)
    {
      SOC_B.Switch = rtb_OCVSOC1;
    }

    SOC_B.Divide = sohd_pct_bcuSoh * cmnc_Q_ratedCp / 100.0F;
  }

  if (SOC_DW.icLoad) {
    SOC_DW.Delay_DSTATE = SOC_B.Switch;
  }

  if (SOC_DW.Delay_DSTATE > socc_pct_battSocUp) {
    socd_pct_bcuSocEo = socc_pct_battSocUp;
  } else if (SOC_DW.Delay_DSTATE < socc_pct_battSocLow) {
    socd_pct_bcuSocEo = socc_pct_battSocLow;
  } else {
    socd_pct_bcuSocEo = SOC_DW.Delay_DSTATE;
  }

  rtb_Gain = -0.5F * ihd_I_battCurr_T2;
  if (dcmd_st_chrgSt_T2 == chrging) {
    uint8_T tmp_4;
    rtb_OCVSOC1 = fabsf(ihd_pct_socCor);
    SOC_DW.UnitDelay_DSTATE_o += rtb_Gain * socc_tm_step;
    rtb_Divide_l = SOC_DW.UnitDelay_DSTATE_o / 3600.0F;
    rtb_MathFunction1_g = floorf(1000.0F * ihd_V_cellUMax);
    if (rtIsNaNF(rtb_MathFunction1_g) || rtIsInfF(rtb_MathFunction1_g)) {
      rtb_MathFunction1_g = 0.0F;
    } else {
      rtb_MathFunction1_g = fmodf(rtb_MathFunction1_g, 65536.0F);
    }

    rtb_Gain3 = (uint16_T)(rtb_MathFunction1_g < 0.0F ? (int32_T)(uint16_T)
      -(int16_T)(uint16_T)-rtb_MathFunction1_g : (int32_T)(uint16_T)
      rtb_MathFunction1_g);
    rtb_LogicalOperator3_i = (rtb_Gain3 > SOC_DW.UnitDelay5_DSTATE);
    if (rtb_LogicalOperator3_i) {
      socd_Q_dMax = rtb_Divide_l - SOC_DW.Delay_DSTATE_f[0];
      SOC_B.LogicalOperator_j = ((socd_Q_dMax - SOC_DW.UnitDelay_DSTATE_i <
        -0.01F) && (SOC_DW.UnitDelay2_DSTATE - SOC_DW.UnitDelay3_DSTATE >= 0.01F));
      for (idxDelay = 0; idxDelay < 9; idxDelay++) {
        SOC_DW.Delay_DSTATE_f[idxDelay] = SOC_DW.Delay_DSTATE_f[idxDelay + 1];
      }

      SOC_DW.Delay_DSTATE_f[9] = rtb_Divide_l;
      SOC_DW.UnitDelay_DSTATE_i = socd_Q_dMax;
      SOC_DW.UnitDelay2_DSTATE = socd_Q_dMax;
      SOC_DW.UnitDelay3_DSTATE = SOC_DW.UnitDelay1_DSTATE_a;
      SOC_DW.UnitDelay1_DSTATE_a = socd_Q_dMax;
    }

    if ((SOC_DW.UnitDelay1_DSTATE_h <= 1.0F) && (rtb_OCVSOC1 >= 1.0F)) {
      tmp_4 = 2U;
    } else {
      tmp_4 = (uint8_T)(SOC_B.LogicalOperator_j && (ihd_V_cellUMax <= 3.4F));
    }

    switch (tmp_4) {
     case 0:
      rtb_Merge3 = 0.0F;
      break;

     case 1:
      rtb_Merge3 = 50.0F - socd_pct_ekfSoc;
      break;

     default:
      rtb_Merge3 = ihd_pct_socCor;
      break;
    }

    rtb_MathFunction1_g = floorf(1000.0F * ihd_V_cellUMin);
    if (rtIsNaNF(rtb_MathFunction1_g) || rtIsInfF(rtb_MathFunction1_g)) {
      rtb_MathFunction1_g = 0.0F;
    } else {
      rtb_MathFunction1_g = fmodf(rtb_MathFunction1_g, 65536.0F);
    }

    rtb_Gain3_l = (uint16_T)(rtb_MathFunction1_g < 0.0F ? (int32_T)(uint16_T)
      -(int16_T)(uint16_T)-rtb_MathFunction1_g : (int32_T)(uint16_T)
      rtb_MathFunction1_g);
    if (rtb_Gain3_l > SOC_DW.UnitDelay5_DSTATE_k) {
      SOC_DW.UnitDelay5_DSTATE_k = rtb_Gain3_l;
      socd_Q_dMin = rtb_Divide_l - SOC_DW.Delay_DSTATE_lp[0];
      for (idxDelay = 0; idxDelay < 9; idxDelay++) {
        SOC_DW.Delay_DSTATE_lp[idxDelay] = SOC_DW.Delay_DSTATE_lp[idxDelay + 1];
      }

      SOC_DW.Delay_DSTATE_lp[9] = rtb_Divide_l;
    }

    if (rtb_LogicalOperator3_i) {
      SOC_DW.UnitDelay5_DSTATE = rtb_Gain3;
    }

    SOC_DW.UnitDelay1_DSTATE_h = rtb_OCVSOC1;
  } else {
    socd_Q_dMin = 0.0F;
    socd_Q_dMax = 0.0F;
    rtb_OCVSOC1 = fabsf(ihd_pct_socCor);
    if ((SOC_DW.UnitDelay1_DSTATE <= 1.0F) && (rtb_OCVSOC1 >= 1.0F)) {
      rtb_Merge3 = ihd_pct_socCor;
    } else {
      rtb_Merge3 = 0.0F;
    }

    SOC_DW.UnitDelay1_DSTATE = rtb_OCVSOC1;
  }

  if (SOC_DW.icLoad_j) {
    SOC_DW.Delay_DSTATE_l = SOC_B.Switch1;
  }

  rtb_MathFunction1_g = fabsf(rtb_Merge3);
  if (rtb_MathFunction1_g >= 1.0F) {
    SOC_DW.Delay_DSTATE_l = (rtb_Gain * socc_tm_step / 36.0F / SOC_B.Divide +
      SOC_DW.Delay_DSTATE_l) + rtb_Merge3;
  } else {
    SOC_DW.Delay_DSTATE_l += rtb_Gain * socc_tm_step / 36.0F / SOC_B.Divide;
  }

  rtb_MatrixConcatenate[0] = 1.0F;
  rtb_MatrixConcatenate[1] = 0.0F;
  if (SOC_DW.icLoad_o) {
    SOC_DW.Delay1_DSTATE = SOC_B.Switch1;
  }

  if (rtb_MathFunction1_g >= 1.0F) {
    SOC_DW.Delay1_DSTATE += rtb_Merge3;
  }

  rtb_OCVSOC1 = expf(-socc_tm_step / look1_iflf_binlcapw(SOC_DW.Delay1_DSTATE,
    &cmnm_pct_soc[0], &cmnm_tm_polar[0], 12U));
  rtb_MatrixConcatenate[2] = 0.0F;
  rtb_MatrixConcatenate[3] = rtb_OCVSOC1;
  for (idxDelay = 0; idxDelay < 2; idxDelay++) {
    tmp[idxDelay] = 0.0F;
    tmp[idxDelay] += SOC_DW.P_Delay_DSTATE[idxDelay];
    rtb_Divide_l = SOC_DW.P_Delay_DSTATE[idxDelay + 2];
    tmp[idxDelay] += rtb_Divide_l * 0.0F;
    tmp[idxDelay + 2] = 0.0F;
    tmp[idxDelay + 2] += SOC_DW.P_Delay_DSTATE[idxDelay] * 0.0F;
    tmp[idxDelay + 2] += rtb_Divide_l * rtb_OCVSOC1;
  }

  rtb_MathFunction1_b[0] = (look1_iflf_binlcapw(SOC_DW.Delay1_DSTATE + 0.01F,
    &cmnm_pct_soc[0], &cmnm_V_ocv[0], 12U) - look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE - 0.01F, &cmnm_pct_soc[0], &cmnm_V_ocv[0], 12U)) /
    SOC_ConstB.Add4_j;
  rtb_MathFunction1_b[1] = 1.0F;
  rtb_Divide_l = 0.0F;
  for (idxDelay = 0; idxDelay < 2; idxDelay++) {
    rtb_Add_ht_tmp_tmp = rtb_MatrixConcatenate[idxDelay + 2];
    rtb_Add6 = (rtb_Add_ht_tmp_tmp * tmp[1] + rtb_MatrixConcatenate[idxDelay] *
                tmp[0]) + 0.0005F;
    rtb_Add_e[idxDelay] = rtb_Add6;
    rtb_Add_ht_tmp_tmp = (rtb_Add_ht_tmp_tmp * tmp[3] +
                          rtb_MatrixConcatenate[idxDelay] * tmp[2]) + 0.0005F;
    rtb_Add_e[idxDelay + 2] = rtb_Add_ht_tmp_tmp;
    rtb_Add6 = rtb_Add6 * rtb_MathFunction1_b[0] + rtb_Add_ht_tmp_tmp;
    rtb_Divide_l += rtb_MathFunction1_b[idxDelay] * rtb_Add6;
    rtb_Divide_f[idxDelay] = rtb_Add6;
  }

  rtb_Add6 = rtb_Divide_f[0] / (rtb_Divide_l + 0.5F);
  rtb_Add_ht_tmp_tmp = rtb_Divide_f[1] / (rtb_Divide_l + 0.5F);
  if (SOC_DW.icLoad_i) {
    SOC_DW.Delay2_DSTATE = 0.0F;
  }

  rtb_Divide_f[1] = (1.0F - rtb_OCVSOC1) * (0.001F * look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE, &cmnm_pct_soc[0], &cmnm_ohm_polar[0], 12U)) *
    rtb_Gain + (0.0F * SOC_DW.Delay1_DSTATE + rtb_OCVSOC1 * SOC_DW.Delay2_DSTATE);
  rtb_Divide_l = ihd_V_cellUMax - ((0.001F * look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE, &cmnm_pct_soc[0], &cmnm_ohm_Ro[0], 12U) * rtb_Gain +
    look1_iflf_binlcapw(SOC_DW.Delay1_DSTATE, &cmnm_pct_soc[0], &cmnm_V_ocv[0],
                        12U)) + rtb_Divide_f[1]);
  rtb_Abs1 = socc_tm_step / SOC_B.Divide * 0.027777778F * rtb_Gain;
  SOC_DW.Delay1_DSTATE = ((0.0F * SOC_DW.Delay2_DSTATE + SOC_DW.Delay1_DSTATE) +
    rtb_Abs1) + rtb_Add6 * rtb_Divide_l;
  rtb_MatrixConcatenate[0] = 1.0F;
  SOC_DW.Delay2_DSTATE = rtb_Add_ht_tmp_tmp * rtb_Divide_l + rtb_Divide_f[1];
  rtb_MatrixConcatenate[1] = 0.0F;
  if (SOC_DW.icLoad_p) {
    SOC_DW.Delay1_DSTATE_a = SOC_B.Switch1;
  }

  if (rtb_MathFunction1_g >= 1.0F) {
    SOC_DW.Delay1_DSTATE_a += rtb_Merge3;
  }

  rtb_OCVSOC1 = expf(-socc_tm_step / look1_iflf_binlcapw(SOC_DW.Delay1_DSTATE_a,
    &cmnm_pct_soc[0], &cmnm_tm_polar[0], 12U));
  rtb_MatrixConcatenate[2] = 0.0F;
  rtb_MatrixConcatenate[3] = rtb_OCVSOC1;
  for (idxDelay = 0; idxDelay < 2; idxDelay++) {
    tmp[idxDelay] = 0.0F;
    tmp[idxDelay] += SOC_DW.P_Delay_DSTATE_k[idxDelay];
    rtb_MathFunction1_g = SOC_DW.P_Delay_DSTATE_k[idxDelay + 2];
    tmp[idxDelay] += rtb_MathFunction1_g * 0.0F;
    tmp[idxDelay + 2] = 0.0F;
    tmp[idxDelay + 2] += SOC_DW.P_Delay_DSTATE_k[idxDelay] * 0.0F;
    tmp[idxDelay + 2] += rtb_MathFunction1_g * rtb_OCVSOC1;
  }

  rtb_MathFunction1_l[0] = (look1_iflf_binlcapw(SOC_DW.Delay1_DSTATE_a + 0.01F,
    &cmnm_pct_soc[0], &cmnm_V_ocv[0], 12U) - look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE_a - 0.01F, &cmnm_pct_soc[0], &cmnm_V_ocv[0], 12U)) /
    SOC_ConstB.Add4;
  rtb_MathFunction1_l[1] = 1.0F;
  rtb_MathFunction1_g = 0.0F;
  for (idxDelay = 0; idxDelay < 2; idxDelay++) {
    rtb_Add_fu_tmp_tmp = rtb_MatrixConcatenate[idxDelay + 2];
    rtb_Merge3 = (rtb_Add_fu_tmp_tmp * tmp[1] + rtb_MatrixConcatenate[idxDelay] *
                  tmp[0]) + 0.0005F;
    rtb_Add_mg[idxDelay] = rtb_Merge3;
    rtb_Add_fu_tmp_tmp = (rtb_Add_fu_tmp_tmp * tmp[3] +
                          rtb_MatrixConcatenate[idxDelay] * tmp[2]) + 0.0005F;
    rtb_Add_mg[idxDelay + 2] = rtb_Add_fu_tmp_tmp;
    rtb_Merge3 = rtb_Merge3 * rtb_MathFunction1_l[0] + rtb_Add_fu_tmp_tmp;
    rtb_MathFunction1_g += rtb_MathFunction1_l[idxDelay] * rtb_Merge3;
    rtb_Divide_f[idxDelay] = rtb_Merge3;
  }

  rtb_Add_fu_tmp_tmp = rtb_Divide_f[0] / (rtb_MathFunction1_g + 0.5F);
  rtb_MathFunction1_g = rtb_Divide_f[1] / (rtb_MathFunction1_g + 0.5F);
  if (SOC_DW.icLoad_a) {
    SOC_DW.Delay2_DSTATE_k = 0.0F;
  }

  rtb_Merge3 = (1.0F - rtb_OCVSOC1) * (0.001F * look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE_a, &cmnm_pct_soc[0], &cmnm_ohm_polar[0], 12U)) *
    rtb_Gain + (0.0F * SOC_DW.Delay1_DSTATE_a + rtb_OCVSOC1 *
                SOC_DW.Delay2_DSTATE_k);
  rtb_OCVSOC1 = ihd_V_cellUMin - ((0.001F * look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE_a, &cmnm_pct_soc[0], &cmnm_ohm_Ro[0], 12U) * rtb_Gain
    + look1_iflf_binlcapw(SOC_DW.Delay1_DSTATE_a, &cmnm_pct_soc[0], &cmnm_V_ocv
    [0], 12U)) + rtb_Merge3);
  SOC_DW.Delay1_DSTATE_a = ((0.0F * SOC_DW.Delay2_DSTATE_k +
    SOC_DW.Delay1_DSTATE_a) + rtb_Abs1) + rtb_Add_fu_tmp_tmp * rtb_OCVSOC1;
  if (SOC_DW.Delay1_DSTATE >= 80.0F) {
    rtb_Abs1 = 1.0F;
  } else if (SOC_DW.Delay1_DSTATE_a <= 20.0F) {
    rtb_Abs1 = 0.0F;
  } else {
    rtb_Abs1 = (SOC_DW.Delay1_DSTATE_a - 20.0F) / ((80.0F -
      (SOC_DW.Delay1_DSTATE - SOC_DW.Delay1_DSTATE_a)) - 20.0F);
  }

  socd_pct_ekfSoc = (1.0F - rtb_Abs1) * SOC_DW.Delay1_DSTATE_a +
    SOC_DW.Delay1_DSTATE * rtb_Abs1;
  rtb_Gain3 = (uint16_T)(SOC_DW.UnitDelay_DSTATE_n + 1U);
  idxDelay = (int32_T)fmod(socc_Nr_judge + 1, 65536.0);
  if ((uint16_T)(SOC_DW.UnitDelay_DSTATE_n + 1U) > (uint16_T)idxDelay) {
    rtb_Gain3 = (uint16_T)idxDelay;
  }

  rtb_LogicalOperator3_i = (rtb_Gain3 >= socc_Nr_judge);
  SOC_Subsystem1(!rtb_LogicalOperator3_i, SOC_DW.Delay_DSTATE_l, socd_pct_ekfSoc,
                 &SOC_B.in1_ix, &SOC_B.in2_fe, &SOC_PrevZCX.Subsystem1);
  if (!rtb_LogicalOperator3_i) {
    socd_pct_battSocEo = (SOC_DW.Delay_DSTATE_l - SOC_B.in1_ix) + SOC_B.in2_fe;
  } else {
    socd_pct_battSocEo = socd_pct_ekfSoc;
  }

  rtPrevAction = SOC_DW.If_ActiveSubsystem;
  SOC_DW.If_ActiveSubsystem = (int8_T)(dcmd_st_chrgSt_T2 != chrging);
  if (SOC_DW.If_ActiveSubsystem == 0) {
    if (rtPrevAction != 0) {
      SOC_DW.UnitDelay_DSTATE_o2 = 0U;
      SOC_DW.UnitDelay_DSTATE_dx = 0U;
      SOC_DW.UnitDelay_DSTATE_fs = false;
      SOC_DW.UnitDelay_DSTATE_h = 0U;
      SOC_DW.UnitDelay_DSTATE_e = false;
    }

    if ((uint16_T)(SOC_DW.UnitDelay_DSTATE_o2 + 1U) <= (uint16_T)idxDelay) {
      SOC_DW.UnitDelay_DSTATE_o2++;
    } else {
      SOC_DW.UnitDelay_DSTATE_o2 = (uint16_T)idxDelay;
    }

    if ((SOC_DW.UnitDelay_DSTATE_o2 > 20) && (!(socd_pct_battSocEo <=
          socc_pct_chrgCCV))) {
      socd_pct_battSocEo = socc_pct_chrgCCV;
    }

    rtb_Gain3_l = (uint16_T)(SOC_DW.UnitDelay_DSTATE_dx + 1U);
    if ((uint16_T)(SOC_DW.UnitDelay_DSTATE_dx + 1U) > (uint16_T)idxDelay) {
      rtb_Gain3_l = (uint16_T)idxDelay;
    }

    rtb_LogicalOperator3_i = ((rtb_Gain3_l >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_fs);
    SOC_Subsystem1(rtb_LogicalOperator3_i, SOC_DW.Delay_DSTATE_l,
                   socd_pct_battSocEo, &SOC_B.in1_i, &SOC_B.in2_f,
                   &SOC_PrevZCX.Subsystem2);
    SOC_DW.UnitDelay_DSTATE_dx = (uint16_T)(ihd_V_cellUMax >=
      look1_iflf_binlcapw(rtb_Gain, &socm_I_chrgCor[0], &socm_V_chrgCor[0], 1U) ?
      (int32_T)rtb_Gain3_l : 0);
    rtb_Gain3_l = (uint16_T)(SOC_DW.UnitDelay_DSTATE_h + 1U);
    if ((uint16_T)(SOC_DW.UnitDelay_DSTATE_h + 1U) > (uint16_T)idxDelay) {
      rtb_Gain3_l = (uint16_T)idxDelay;
    }

    SOC_DW.UnitDelay_DSTATE_h = (uint16_T)(ihd_V_cellUMax >= cmnc_V_chrgLim ?
      (int32_T)rtb_Gain3_l : 0);
    SOC_DW.UnitDelay_DSTATE_fs = SOC_DW.UnitDelay_DSTATE_e;
    SOC_DW.UnitDelay_DSTATE_e = ((rtb_Gain3_l >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_fs);
    if (SOC_DW.UnitDelay_DSTATE_e) {
      socd_pct_battSocEo = socc_pct_battSocUp;
    } else {
      if (rtb_LogicalOperator3_i) {
        socd_pct_battSocEo = (SOC_DW.Delay_DSTATE_l - SOC_B.in1_i) + fmaxf
          (socc_pct_chrgCCV, SOC_B.in2_f);
      }

      if (!(socd_pct_battSocEo <= (real32_T)(socc_pct_battSocUp - 0.1))) {
        socd_pct_battSocEo = (real32_T)(socc_pct_battSocUp - 0.1);
      }
    }

    SOC_DW.UnitDelay_DSTATE_fs = rtb_LogicalOperator3_i;
  } else {
    uint16_T rtb_Switch_l;
    if (rtPrevAction != 1) {
      SOC_DW.UnitDelay_DSTATE_c = 0.0;
      SOC_DW.UnitDelay_DSTATE_p = 0U;
      SOC_DW.UnitDelay_DSTATE_ca = false;
      SOC_DW.UnitDelay_DSTATE_ol = 0U;
      SOC_DW.UnitDelay_DSTATE_f = false;
    }

    if (SOC_DW.UnitDelay_DSTATE_c + 1.0 <= SOC_ConstB.Add_i) {
      SOC_DW.UnitDelay_DSTATE_c++;
    } else {
      SOC_DW.UnitDelay_DSTATE_c = SOC_ConstB.Add_i;
    }

    if ((SOC_DW.UnitDelay_DSTATE_c > 20.0) && (!(socd_pct_battSocEo >=
          socc_pct_disChrgCCV))) {
      socd_pct_battSocEo = socc_pct_disChrgCCV;
    }

    rtb_Gain3_l = (uint16_T)(SOC_DW.UnitDelay_DSTATE_p + 1U);
    if ((uint16_T)(SOC_DW.UnitDelay_DSTATE_p + 1U) > socc_Nr_judge) {
      rtb_Gain3_l = socc_Nr_judge;
    }

    rtb_LogicalOperator3_i = ((rtb_Gain3_l >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_ca);
    SOC_Subsystem1(rtb_LogicalOperator3_i, SOC_DW.Delay_DSTATE_l,
                   socd_pct_battSocEo, &SOC_B.in1, &SOC_B.in2,
                   &SOC_PrevZCX.Subsystem2_e);
    rtb_Switch_l = (uint16_T)(SOC_DW.UnitDelay_DSTATE_ol + 1U);
    if ((uint16_T)(SOC_DW.UnitDelay_DSTATE_ol + 1U) > socc_Nr_judge) {
      rtb_Switch_l = socc_Nr_judge;
    }

    SOC_DW.UnitDelay_DSTATE_ol = (uint16_T)(ihd_V_cellUMin <=
      look1_iflf_binlcapw(rtb_Gain, &socm_I_disChrgCor[0], &socm_V_emptyV[0], 1U)
      ? (int32_T)rtb_Switch_l : 0);
    SOC_DW.UnitDelay_DSTATE_p = (uint16_T)(ihd_V_cellUMin <= look1_iflf_binlcapw
      (rtb_Gain, &socm_I_disChrgCor[0], &socm_V_disChrgCor[0], 1U) ? (int32_T)
      rtb_Gain3_l : 0);
    SOC_DW.UnitDelay_DSTATE_ca = SOC_DW.UnitDelay_DSTATE_f;
    SOC_DW.UnitDelay_DSTATE_f = ((rtb_Switch_l >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_ca);
    if (SOC_DW.UnitDelay_DSTATE_f) {
      socd_pct_battSocEo = socc_pct_battSocLow;
    } else {
      if (rtb_LogicalOperator3_i) {
        socd_pct_battSocEo = (SOC_DW.Delay_DSTATE_l - SOC_B.in1) + fminf
          (socc_pct_disChrgCCV, SOC_B.in2);
      }

      if (!(socd_pct_battSocEo >= (real32_T)(socc_pct_battSocLow + 0.1))) {
        socd_pct_battSocEo = (real32_T)(socc_pct_battSocLow + 0.1);
      }
    }

    SOC_DW.UnitDelay_DSTATE_ca = rtb_LogicalOperator3_i;
  }

  rtb_Abs1 = 1.0F - rtb_Add_fu_tmp_tmp * rtb_MathFunction1_l[0];
  tmp_2 = 0.0F - rtb_MathFunction1_g * rtb_MathFunction1_l[0];
  tmp_0 = 1.0F - rtb_Add6 * rtb_MathFunction1_b[0];
  tmp_1 = 0.0F - rtb_Add_ht_tmp_tmp * rtb_MathFunction1_b[0];
  for (idxDelay = 0; idxDelay < 2; idxDelay++) {
    int32_T P_Delay_DSTATE_k_tmp;
    real32_T P_Delay_DSTATE_k_tmp_0;
    real32_T P_Delay_DSTATE_k_tmp_1;
    P_Delay_DSTATE_k_tmp = idxDelay << 1;
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp] = 0.0F;
    P_Delay_DSTATE_k_tmp_0 = rtb_Add_mg[P_Delay_DSTATE_k_tmp];
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp] += P_Delay_DSTATE_k_tmp_0 *
      rtb_Abs1;
    P_Delay_DSTATE_k_tmp_1 = rtb_Add_mg[P_Delay_DSTATE_k_tmp + 1];
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp] += (0.0F - rtb_Add_fu_tmp_tmp)
      * P_Delay_DSTATE_k_tmp_1;
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp + 1] = 0.0F;
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp + 1] += P_Delay_DSTATE_k_tmp_0 *
      tmp_2;
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp + 1] += (1.0F -
      rtb_MathFunction1_g) * P_Delay_DSTATE_k_tmp_1;
    SOC_DW.P_Delay_DSTATE[P_Delay_DSTATE_k_tmp] = 0.0F;
    P_Delay_DSTATE_k_tmp_0 = rtb_Add_e[P_Delay_DSTATE_k_tmp];
    SOC_DW.P_Delay_DSTATE[P_Delay_DSTATE_k_tmp] += P_Delay_DSTATE_k_tmp_0 *
      tmp_0;
    P_Delay_DSTATE_k_tmp_1 = rtb_Add_e[P_Delay_DSTATE_k_tmp + 1];
    SOC_DW.P_Delay_DSTATE[P_Delay_DSTATE_k_tmp] += (0.0F - rtb_Add6) *
      P_Delay_DSTATE_k_tmp_1;
    SOC_DW.P_Delay_DSTATE[P_Delay_DSTATE_k_tmp + 1] = 0.0F;
    SOC_DW.P_Delay_DSTATE[P_Delay_DSTATE_k_tmp + 1] += P_Delay_DSTATE_k_tmp_0 *
      tmp_1;
    SOC_DW.P_Delay_DSTATE[P_Delay_DSTATE_k_tmp + 1] += (1.0F -
      rtb_Add_ht_tmp_tmp) * P_Delay_DSTATE_k_tmp_1;
  }

  if (SOC_DW.Delay_DSTATE_l > socc_pct_battSocUp) {
    socd_pct_ahSoc = socc_pct_battSocUp;
  } else if (SOC_DW.Delay_DSTATE_l < socc_pct_battSocLow) {
    socd_pct_ahSoc = socc_pct_battSocLow;
  } else {
    socd_pct_ahSoc = SOC_DW.Delay_DSTATE_l;
  }

  if (SOC_DW.icLoad_l) {
    SOC_DW.Delay1_DSTATE_n = socd_pct_battSocEo;
  }

  if ((uint16_T)(SOC_DW.UnitDelay_DSTATE_m + 1U) <= SOC_ConstB.Add_j) {
    rtb_Switch_n = (uint16_T)(SOC_DW.UnitDelay_DSTATE_m + 1U);
  } else {
    rtb_Switch_n = SOC_ConstB.Add_j;
  }

  tmp_3 = floor((real_T)(fabsf(rtb_Gain) < 1.0F) * rtb_Switch_n);
  if (rtIsNaN(tmp_3) || rtIsInf(tmp_3)) {
    tmp_3 = 0.0;
  } else {
    tmp_3 = fmod(tmp_3, 65536.0);
  }

  SOC_DW.UnitDelay_DSTATE_m = (uint16_T)(tmp_3 < 0.0 ? (int32_T)(uint16_T)
    -(int16_T)(uint16_T)-tmp_3 : (int32_T)(uint16_T)tmp_3);
  if (SOC_DW.icLoad_m) {
    SOC_DW.Delay2_DSTATE_b = dcmd_st_chrgSt_T2;
  }

  if (SOC_DW.UnitDelay_DSTATE_d + 1.0 <= SOC_ConstB.Add_m) {
    SOC_DW.UnitDelay_DSTATE_d++;
  } else {
    SOC_DW.UnitDelay_DSTATE_d = SOC_ConstB.Add_m;
  }

  if ((SOC_DW.UnitDelay_DSTATE_d == 1.0) || ((SOC_DW.Delay2_DSTATE_b == chrging)
       && (dcmd_st_chrgSt_T2 != chrging)) || ((SOC_DW.Delay2_DSTATE_b != chrging)
       && (dcmd_st_chrgSt_T2 == chrging)) || (fabsf(socd_pct_battSocEo -
        SOC_DW.Delay1_DSTATE_n) > 2.0F) || (rtb_Switch_n > (uint16_T)(5U *
        socc_Nr_judge))) {
    SOC_DW.socd_pct_battSoc0_Delay_DSTATE = socd_pct_battSocEo;
    SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE = socd_pct_bcuSocEo;
  }

  rtPrevAction = SOC_DW.If_ActiveSubsystem_j;
  SOC_DW.If_ActiveSubsystem_j = (int8_T)(dcmd_st_chrgSt_T2 != chrging);
  if (SOC_DW.If_ActiveSubsystem_j == 0) {
    if (rtPrevAction != 0) {
      SOC_DW.icLoad_g = true;
      SOC_DW.UnitDelay_DSTATE_b = 0.0F;
    }

    if (socd_pct_battSocEo <= SOC_DW.socd_pct_battSoc0_Delay_DSTATE) {
      rtb_Gain = SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE;
    } else if (socd_pct_battSocEo >= socc_pct_battSocUp) {
      rtb_Gain = socc_pct_battSocUp;
    } else {
      rtb_Gain = fminf(socc_pct_battSocUp, fminf(5.0F, fabsf
        (SOC_DW.socd_pct_battSoc0_Delay_DSTATE -
         SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE)) + fmaxf
                       (SOC_DW.socd_pct_battSoc0_Delay_DSTATE,
                        SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE));
      if (socd_pct_battSocEo > rtb_Gain) {
        rtb_Add6 = socc_pct_battSocUp - rtb_Gain;
        rtb_Gain += (socd_pct_battSocEo - rtb_Gain) * rtb_Add6 / fmaxf(0.1F,
          rtb_Add6);
      } else {
        rtb_Gain = (socd_pct_battSocEo - SOC_DW.socd_pct_battSoc0_Delay_DSTATE) *
          (rtb_Gain - SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE) / fmaxf(0.1F,
          rtb_Gain - SOC_DW.socd_pct_battSoc0_Delay_DSTATE) +
          SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE;
      }
    }

    if (SOC_DW.icLoad_g) {
      SOC_DW.Delay_DSTATE_h0 = rtb_Gain;
    }

    if (SOC_DW.Delay_DSTATE_h0 >= SOC_DW.UnitDelay_DSTATE_b) {
      SOC_DW.UnitDelay_DSTATE_b = SOC_DW.Delay_DSTATE_h0;
    }

    if (SOC_DW.UnitDelay_DSTATE_e) {
      SOC_DW.Delay_DSTATE = socc_pct_battSocUp;
    } else if (SOC_DW.UnitDelay_DSTATE_b <= (real32_T)(socc_pct_battSocUp - 0.1))
    {
      SOC_DW.Delay_DSTATE = SOC_DW.UnitDelay_DSTATE_b;
    } else {
      SOC_DW.Delay_DSTATE = (real32_T)(socc_pct_battSocUp - 0.1);
    }

    rtb_Add6 = rtb_Gain - SOC_DW.Delay_DSTATE_h0;
    if (rtb_Add6 > socc_pct_fitRate) {
      SOC_IfActionSubsystem(SOC_DW.Delay_DSTATE_h0, &SOC_DW.Delay_DSTATE_h0,
                            socc_pct_fitRate);
    } else if (rtb_Add6 < -socc_pct_fitRate) {
      SOC_IfActionSubsystem1(SOC_DW.Delay_DSTATE_h0, &SOC_DW.Delay_DSTATE_h0,
        socc_pct_fitRate);
    } else {
      SOC_DW.Delay_DSTATE_h0 = rtb_Gain;
    }

    SOC_DW.icLoad_g = false;
  } else {
    SOC_DW.icLoad_n = ((rtPrevAction != 1) || SOC_DW.icLoad_n);
    if (socd_pct_battSocEo <= socc_pct_battSocLow) {
      rtb_Gain = socc_pct_battSocLow;
    } else if (socd_pct_battSocEo >= SOC_DW.socd_pct_battSoc0_Delay_DSTATE) {
      rtb_Gain = SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE;
    } else {
      rtb_Gain = fmaxf(fminf(SOC_DW.socd_pct_battSoc0_Delay_DSTATE,
        SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE) - fminf(5.0F, fabsf
        (SOC_DW.socd_pct_battSoc0_Delay_DSTATE -
         SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE)), socc_pct_battSocLow);
      if (socd_pct_battSocEo > rtb_Gain) {
        rtb_Gain += (socd_pct_battSocEo - rtb_Gain) *
          (SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE - rtb_Gain) / fmaxf(0.1F,
          SOC_DW.socd_pct_battSoc0_Delay_DSTATE - rtb_Gain);
      } else {
        rtb_Gain -= socc_pct_battSocLow;
        rtb_Gain = (socd_pct_battSocEo - socc_pct_battSocLow) * rtb_Gain / fmaxf
          (0.1F, rtb_Gain) + socc_pct_battSocLow;
      }
    }

    if (SOC_DW.icLoad_n) {
      SOC_DW.Delay_DSTATE_h = rtb_Gain;
    }

    if (SOC_DW.UnitDelay_DSTATE_f) {
      SOC_DW.Delay_DSTATE = socc_pct_battSocLow;
    } else if (SOC_DW.Delay_DSTATE_h >= (real32_T)(socc_pct_battSocLow + 0.1)) {
      SOC_DW.Delay_DSTATE = SOC_DW.Delay_DSTATE_h;
    } else {
      SOC_DW.Delay_DSTATE = (real32_T)(socc_pct_battSocLow + 0.1);
    }

    rtb_Add6 = rtb_Gain - SOC_DW.Delay_DSTATE_h;
    if (rtb_Add6 > socc_pct_fitRate) {
      SOC_IfActionSubsystem(SOC_DW.Delay_DSTATE_h, &SOC_DW.Delay_DSTATE_h,
                            socc_pct_fitRate);
    } else if (rtb_Add6 < -socc_pct_fitRate) {
      SOC_IfActionSubsystem1(SOC_DW.Delay_DSTATE_h, &SOC_DW.Delay_DSTATE_h,
        socc_pct_fitRate);
    } else {
      SOC_DW.Delay_DSTATE_h = rtb_Gain;
    }

    SOC_DW.icLoad_n = false;
  }

  socd_pct_battSoc = socd_pct_battSocEo;
  socd_pct_bcuSoc = socd_pct_bcuSocEo;
  socd_flg_EEsave = ((fabsf(socd_pct_battSocEo -
    SOC_DW.socd_flg_EEsave_Delay_DSTATE) > 1.0F) || (fabsf(socd_pct_bcuSocEo -
    SOC_DW.socd_flg_EEsave_Delay1_DSTATE) > 1.0F));
  if (socd_flg_EEsave) {
    SOC_DW.socd_flg_EEsave_Delay1_DSTATE = socd_pct_bcuSocEo;
    SOC_DW.socd_flg_EEsave_Delay_DSTATE = socd_pct_battSocEo;
  }

  SOC_DW.UnitDelay_DSTATE_n = (uint16_T)((!(fabsf(rtb_OCVSOC1) > 0.05F)) &&
    (!(fabsf(rtb_Divide_l) > 0.05F)) ? (int32_T)rtb_Gain3 : 0);
  SOC_DW.icLoad = false;
  SOC_DW.icLoad_j = false;
  SOC_DW.icLoad_o = false;
  SOC_DW.icLoad_i = false;
  SOC_DW.icLoad_p = false;
  SOC_DW.icLoad_a = false;
  SOC_DW.Delay2_DSTATE_k = rtb_MathFunction1_g * rtb_OCVSOC1 + rtb_Merge3;
  SOC_DW.icLoad_l = false;
  SOC_DW.Delay1_DSTATE_n = socd_pct_battSocEo;
  SOC_DW.icLoad_m = false;
  SOC_DW.Delay2_DSTATE_b = dcmd_st_chrgSt_T2;
}

void SOC_initialize(const char_T **rt_errorStatus)
{
  RT_MODEL_SOC_T *const SOC_M = &(SOC_MdlrefDW.rtm);
  rt_InitInfAndNaN(sizeof(real_T));
  rtmSetErrorStatusPointer(SOC_M, rt_errorStatus);
  SOC_PrevZCX.Subsystem2_e.Subsystem1_Trig_ZCE = POS_ZCSIG;
  SOC_PrevZCX.Subsystem2.Subsystem1_Trig_ZCE = POS_ZCSIG;
  SOC_PrevZCX.Subsystem1.Subsystem1_Trig_ZCE = POS_ZCSIG;
}