#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_FristEn_Init(DW_FristEn_SOC_T *localDW)
{
  localDW->UnitDelay_DSTATE = true;
}

void SOC_FristEn(boolean_T *rty_Out1, DW_FristEn_SOC_T *localDW)
{
  *rty_Out1 = localDW->UnitDelay_DSTATE;
  localDW->UnitDelay_DSTATE = false;
}

void SOC_Subsystem(real32_T rtu_x, real32_T rtu_xdat, real32_T rtu_xdat_o,
                   real32_T rtu_xdat_d, real32_T rtu_ydat, real32_T rtu_ydat_e,
                   real32_T *rty_y)
{
  if (rtu_x <= rtu_xdat_d) {
    *rty_y = rtu_ydat_e;
  } else if (rtu_x >= rtu_xdat) {
    *rty_y = rtu_ydat;
  } else if (rtu_x > rtu_xdat_o) {
    *rty_y = (rtu_x - rtu_xdat_o) * (rtu_ydat - rtu_xdat_o) / fmaxf(0.1F,
      rtu_xdat - rtu_xdat_o) + rtu_xdat_o;
  } else {
    *rty_y = (rtu_x - rtu_xdat_d) * (rtu_xdat_o - rtu_ydat_e) / fmaxf(0.1F,
      rtu_xdat_o - rtu_xdat_d) + rtu_ydat_e;
  }
}

void SOC_UpLim(real32_T rtu_In1, real32_T *rty_Out1, real32_T rtp_m, real_T
               rtp_n)
{
  real32_T rtb_Add_e;
  rtb_Add_e = (real32_T)(rtp_m + rtp_n);
  if (rtu_In1 <= rtb_Add_e) {
    *rty_Out1 = rtu_In1;
  } else {
    *rty_Out1 = rtb_Add_e;
  }
}

void SOC_fit_Init(DW_fit_SOC_T *localDW)
{
  localDW->icLoad = true;
}

void SOC_fit_Reset(DW_fit_SOC_T *localDW)
{
  localDW->icLoad = true;
}

void SOC_fit(real32_T rtu_data, real32_T *rty_Fitdata, real32_T rtp_m,
             DW_fit_SOC_T *localDW)
{
  real32_T rtb_Add6;
  uint8_T rtb_Switch_d;
  if (localDW->icLoad) {
    localDW->Delay_DSTATE = rtu_data;
  }

  *rty_Fitdata = localDW->Delay_DSTATE;
  rtb_Add6 = rtu_data - *rty_Fitdata;
  if (rtb_Add6 > rtp_m) {
    rtb_Switch_d = 2U;
  } else {
    rtb_Switch_d = (uint8_T)(rtb_Add6 < -rtp_m);
  }

  localDW->icLoad = false;
  if (rtb_Switch_d == 2) {
    localDW->Delay_DSTATE = rtp_m + *rty_Fitdata;
  } else if (rtb_Switch_d == 1) {
    localDW->Delay_DSTATE = -rtp_m + *rty_Fitdata;
  } else {
    localDW->Delay_DSTATE = rtu_data;
  }
}

void SOC_LowLim(real32_T rtu_In1, real32_T *rty_Out1, real32_T rtp_m, real_T
                rtp_n)
{
  real32_T rtb_Add_g;
  rtb_Add_g = (real32_T)(rtp_m + rtp_n);
  if (rtu_In1 >= rtb_Add_g) {
    *rty_Out1 = rtu_In1;
  } else {
    *rty_Out1 = rtb_Add_g;
  }
}

void SOC_UpLim_n(uint16_T rtu_In1, uint16_T *rty_Out1, uint16_T rtp_m, real_T
                 rtp_n)
{
  real_T tmp;
  uint16_T rtb_Add_g;
  tmp = floor((real_T)rtp_m + rtp_n);
  if (rtIsNaN(tmp) || rtIsInf(tmp)) {
    tmp = 0.0;
  } else {
    tmp = fmod(tmp, 65536.0);
  }

  rtb_Add_g = (uint16_T)(tmp < 0.0 ? (int32_T)(uint16_T)-(int16_T)(uint16_T)-tmp
    : (int32_T)(uint16_T)tmp);
  if (rtu_In1 <= rtb_Add_g) {
    *rty_Out1 = rtu_In1;
  } else {
    *rty_Out1 = rtb_Add_g;
  }
}

void SOC_Conditionalcounter1_Reset(DW_Conditionalcounter1_SOC_T *localDW)
{
  localDW->UnitDelay_DSTATE = 0U;
}

void SOC_Conditionalcounter1(uint16_T *rty_Nr, uint16_T rtp_n,
  DW_Conditionalcounter1_SOC_T *localDW)
{
  SOC_UpLim_n((uint16_T)(localDW->UnitDelay_DSTATE + 1U), rty_Nr, rtp_n, 1.0);
}

void SOC_Conditionalcounter1_Update(boolean_T rtu_Flg, uint16_T *rty_Nr,
  DW_Conditionalcounter1_SOC_T *localDW)
{
  localDW->UnitDelay_DSTATE = (uint16_T)(rtu_Flg ? (int32_T)*rty_Nr : 0);
}

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_TimeCounter_Reset(DW_TimeCounter_SOC_T *localDW)
{
  localDW->UnitDelay_DSTATE = 0.0;
}

void SOC_TimeCounter(real_T *rty_Nr, real_T rtp_n, DW_TimeCounter_SOC_T *localDW)
{
  if (localDW->UnitDelay_DSTATE + 1.0 <= rtp_n + 1.0) {
    *rty_Nr = localDW->UnitDelay_DSTATE + 1.0;
  } else {
    *rty_Nr = rtp_n + 1.0;
  }

  localDW->UnitDelay_DSTATE = *rty_Nr;
}

void SOC_Conditionalcounter_Reset(DW_Conditionalcounter_SOC_T *localDW)
{
  localDW->UnitDelay_DSTATE = 0U;
}

void SOC_Conditionalcounter(uint16_T *rty_Nr, uint16_T rtp_n,
  DW_Conditionalcounter_SOC_T *localDW)
{
  SOC_UpLim_n((uint16_T)(localDW->UnitDelay_DSTATE + 1U), rty_Nr, rtp_n, 0.0);
}

void SOC_Conditionalcounter_Update(boolean_T rtu_Flg, uint16_T *rty_Nr,
  DW_Conditionalcounter_SOC_T *localDW)
{
  localDW->UnitDelay_DSTATE = (uint16_T)(rtu_Flg ? (int32_T)*rty_Nr : 0);
}

void SOC_Init(void)
{
  SOC_DW.If_ActiveSubsystem = -1;
  SOC_DW.If_ActiveSubsystem_j = -1;
  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_FristEn_Init(&SOC_DW.FristEn);
  SOC_FristEn_Init(&SOC_DW.FristEn_n);
  SOC_fit_Init(&SOC_DW.fit);
  SOC_fit_Init(&SOC_DW.fit_j);
}

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

void SOC(void)
{
  real_T rtb_Switch;
  int32_T Switch;
  real32_T rtb_Add_ij[4];
  real32_T rtb_Add_ob[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_Abs;
  real32_T rtb_Abs1;
  real32_T rtb_Add_f_tmp_tmp;
  real32_T rtb_Delay_j;
  real32_T rtb_Divide_l;
  real32_T rtb_MathFunction1_g;
  real32_T rtb_Merge3;
  real32_T rtb_OCVSOC1;
  real32_T rtb_Switch2_l;
  real32_T rtb_Switch7;
  real32_T tmp_0;
  real32_T tmp_1;
  uint16_T Switch_cz;
  uint16_T rtb_Gain3;
  uint16_T rtb_Gain3_l;
  int8_T rtPrevAction;
  boolean_T rtb_RelationalOperator_pu;
  SOC_FristEn(&rtb_RelationalOperator_pu, &SOC_DW.FristEn);
  if (rtb_RelationalOperator_pu) {
    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_d = rtb_OCVSOC1;
    } else {
      SOC_B.Switch1 = socd_pct_battSocEi;
      SOC_B.Switch_d = socd_pct_bcuSocEi;
    }

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

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

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

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

  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_OCVSOC1 = -0.5F * ihd_I_battCurr_T2;
  if (dcmd_st_chrgSt_T2 == chrging) {
    uint8_T tmp_2;
    rtb_Abs = fabsf(ihd_pct_socCor);
    SOC_DW.UnitDelay_DSTATE_o += rtb_OCVSOC1 * 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_RelationalOperator_pu = (rtb_Gain3 > SOC_DW.UnitDelay5_DSTATE);
    if (rtb_RelationalOperator_pu) {
      SOC_B.SignalConversion = rtb_Divide_l - SOC_DW.Delay_DSTATE_f[0];
      SOC_B.LogicalOperator_j = ((SOC_B.SignalConversion -
        SOC_DW.UnitDelay_DSTATE_i < -0.01F) && (SOC_DW.UnitDelay2_DSTATE -
        SOC_DW.UnitDelay3_DSTATE >= 0.01F));
      for (Switch = 0; Switch < 9; Switch++) {
        SOC_DW.Delay_DSTATE_f[Switch] = SOC_DW.Delay_DSTATE_f[Switch + 1];
      }

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

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

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

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

     default:
      rtb_Merge3 = ihd_pct_socCor;
      break;
    }

    socd_Q_dMax = SOC_B.SignalConversion;
    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_B.SignalConversion_b = rtb_Divide_l - SOC_DW.Delay_DSTATE_lp[0];
      for (Switch = 0; Switch < 9; Switch++) {
        SOC_DW.Delay_DSTATE_lp[Switch] = SOC_DW.Delay_DSTATE_lp[Switch + 1];
      }

      SOC_DW.Delay_DSTATE_lp[9] = rtb_Divide_l;
      SOC_DW.UnitDelay5_DSTATE_k = rtb_Gain3_l;
    }

    socd_Q_dMin = SOC_B.SignalConversion_b;
    if (rtb_RelationalOperator_pu) {
      SOC_DW.UnitDelay5_DSTATE = rtb_Gain3;
    }

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

    SOC_DW.UnitDelay1_DSTATE = rtb_Abs;
  }

  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_OCVSOC1 * socc_tm_step / 36.0F / SOC_B.Divide +
      SOC_DW.Delay_DSTATE_l) + rtb_Merge3;
  } else {
    SOC_DW.Delay_DSTATE_l += rtb_OCVSOC1 * 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_Abs = 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_Abs;
  for (Switch = 0; Switch < 2; Switch++) {
    tmp[Switch] = 0.0F;
    tmp[Switch] += SOC_DW.P_Delay_DSTATE[Switch];
    rtb_Divide_l = SOC_DW.P_Delay_DSTATE[Switch + 2];
    tmp[Switch] += rtb_Divide_l * 0.0F;
    tmp[Switch + 2] = 0.0F;
    tmp[Switch + 2] += SOC_DW.P_Delay_DSTATE[Switch] * 0.0F;
    tmp[Switch + 2] += rtb_Divide_l * rtb_Abs;
  }

  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 (Switch = 0; Switch < 2; Switch++) {
    rtb_Switch7 = rtb_MatrixConcatenate[Switch + 2];
    rtb_Delay_j = (rtb_Switch7 * tmp[1] + rtb_MatrixConcatenate[Switch] * tmp[0])
      + 0.0005F;
    rtb_Add_ob[Switch] = rtb_Delay_j;
    rtb_Switch7 = (rtb_Switch7 * tmp[3] + rtb_MatrixConcatenate[Switch] * tmp[2])
      + 0.0005F;
    rtb_Add_ob[Switch + 2] = rtb_Switch7;
    rtb_Delay_j = rtb_Delay_j * rtb_MathFunction1_b[0] + rtb_Switch7;
    rtb_Divide_l += rtb_MathFunction1_b[Switch] * rtb_Delay_j;
    rtb_Divide_f[Switch] = rtb_Delay_j;
  }

  rtb_Delay_j = rtb_Divide_f[0] / (rtb_Divide_l + 0.5F);
  rtb_Switch7 = 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_Abs) * (0.001F * look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE, &cmnm_pct_soc[0], &cmnm_ohm_polar[0], 12U)) *
    rtb_OCVSOC1 + (0.0F * SOC_DW.Delay1_DSTATE + rtb_Abs * 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_OCVSOC1
    + 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_OCVSOC1;
  SOC_DW.Delay1_DSTATE = ((0.0F * SOC_DW.Delay2_DSTATE + SOC_DW.Delay1_DSTATE) +
    rtb_Abs1) + rtb_Delay_j * rtb_Divide_l;
  rtb_MatrixConcatenate[0] = 1.0F;
  SOC_DW.Delay2_DSTATE = rtb_Switch7 * 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_Abs = 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_Abs;
  for (Switch = 0; Switch < 2; Switch++) {
    tmp[Switch] = 0.0F;
    tmp[Switch] += SOC_DW.P_Delay_DSTATE_k[Switch];
    rtb_MathFunction1_g = SOC_DW.P_Delay_DSTATE_k[Switch + 2];
    tmp[Switch] += rtb_MathFunction1_g * 0.0F;
    tmp[Switch + 2] = 0.0F;
    tmp[Switch + 2] += SOC_DW.P_Delay_DSTATE_k[Switch] * 0.0F;
    tmp[Switch + 2] += rtb_MathFunction1_g * rtb_Abs;
  }

  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 (Switch = 0; Switch < 2; Switch++) {
    rtb_Add_f_tmp_tmp = rtb_MatrixConcatenate[Switch + 2];
    rtb_Merge3 = (rtb_Add_f_tmp_tmp * tmp[1] + rtb_MatrixConcatenate[Switch] *
                  tmp[0]) + 0.0005F;
    rtb_Add_ij[Switch] = rtb_Merge3;
    rtb_Add_f_tmp_tmp = (rtb_Add_f_tmp_tmp * tmp[3] +
                         rtb_MatrixConcatenate[Switch] * tmp[2]) + 0.0005F;
    rtb_Add_ij[Switch + 2] = rtb_Add_f_tmp_tmp;
    rtb_Merge3 = rtb_Merge3 * rtb_MathFunction1_l[0] + rtb_Add_f_tmp_tmp;
    rtb_MathFunction1_g += rtb_MathFunction1_l[Switch] * rtb_Merge3;
    rtb_Divide_f[Switch] = rtb_Merge3;
  }

  rtb_Add_f_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_Abs) * (0.001F * look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE_a, &cmnm_pct_soc[0], &cmnm_ohm_polar[0], 12U)) *
    rtb_OCVSOC1 + (0.0F * SOC_DW.Delay1_DSTATE_a + rtb_Abs *
                   SOC_DW.Delay2_DSTATE_k);
  rtb_Abs = ihd_V_cellUMin - ((0.001F * look1_iflf_binlcapw
    (SOC_DW.Delay1_DSTATE_a, &cmnm_pct_soc[0], &cmnm_ohm_Ro[0], 12U) *
    rtb_OCVSOC1 + 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_f_tmp_tmp * rtb_Abs;
  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;
  SOC_Conditionalcounter1(&rtb_Gain3, socc_Nr_judge, &SOC_DW.Conditionalcounter1);
  rtb_RelationalOperator_pu = (rtb_Gain3 >= socc_Nr_judge);
  SOC_Subsystem1(!rtb_RelationalOperator_pu, SOC_DW.Delay_DSTATE_l,
                 socd_pct_ekfSoc, &SOC_B.in1_ix, &SOC_B.in2_fe,
                 &SOC_PrevZCX.Subsystem1);
  if (!rtb_RelationalOperator_pu) {
    rtb_Abs1 = (SOC_DW.Delay_DSTATE_l - SOC_B.in1_ix) + SOC_B.in2_fe;
  } else {
    rtb_Abs1 = 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_fs = false;
      SOC_DW.UnitDelay_DSTATE_e = false;
      SOC_TimeCounter_Reset(&SOC_DW.TimeCounter);
      SOC_Conditionalcounter1_Reset(&SOC_DW.Conditionalcounter_i);
      SOC_Conditionalcounter1_Reset(&SOC_DW.Conditionalcounter1_j);
    }

    SOC_TimeCounter(&rtb_Switch, 20.0, &SOC_DW.TimeCounter);
    if (rtb_Switch > 20.0) {
      SOC_UpLim(rtb_Abs1, &rtb_Switch2_l, socc_pct_chrgCCV, 0.0);
    } else {
      rtb_Switch2_l = rtb_Abs1;
    }

    SOC_Conditionalcounter1(&rtb_Gain3_l, socc_Nr_judge,
      &SOC_DW.Conditionalcounter_i);
    rtb_RelationalOperator_pu = ((rtb_Gain3_l >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_fs);
    SOC_Subsystem1(rtb_RelationalOperator_pu, SOC_DW.Delay_DSTATE_l,
                   rtb_Switch2_l, &SOC_B.in1_i, &SOC_B.in2_f,
                   &SOC_PrevZCX.Subsystem2);
    SOC_Conditionalcounter1(&Switch_cz, socc_Nr_judge,
      &SOC_DW.Conditionalcounter1_j);
    SOC_DW.UnitDelay_DSTATE_fs = SOC_DW.UnitDelay_DSTATE_e;
    SOC_DW.UnitDelay_DSTATE_e = ((Switch_cz >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_fs);
    if (SOC_DW.UnitDelay_DSTATE_e) {
      socd_pct_battSocEo = socc_pct_battSocUp;
    } else {
      if (rtb_RelationalOperator_pu) {
        rtb_Switch2_l = (SOC_DW.Delay_DSTATE_l - SOC_B.in1_i) + fmaxf
          (socc_pct_chrgCCV, SOC_B.in2_f);
      }

      SOC_UpLim(rtb_Switch2_l, &socd_pct_battSocEo, socc_pct_battSocUp, -0.1);
    }

    SOC_Conditionalcounter1_Update(ihd_V_cellUMax >= look1_iflf_binlcapw
      (rtb_OCVSOC1, &socm_I_chrgCor[0], &socm_V_chrgCor[0], 1U), &rtb_Gain3_l,
      &SOC_DW.Conditionalcounter_i);
    SOC_DW.UnitDelay_DSTATE_fs = rtb_RelationalOperator_pu;
    SOC_Conditionalcounter1_Update(ihd_V_cellUMax >= cmnc_V_chrgLim, &Switch_cz,
      &SOC_DW.Conditionalcounter1_j);
  } else {
    if (rtPrevAction != 1) {
      SOC_DW.UnitDelay_DSTATE_c = false;
      SOC_DW.UnitDelay_DSTATE_f = false;
      SOC_TimeCounter_Reset(&SOC_DW.TimeCounter_c);
      SOC_Conditionalcounter_Reset(&SOC_DW.Conditionalcounter1_f);
      SOC_Conditionalcounter_Reset(&SOC_DW.Conditionalcounter_n);
    }

    SOC_TimeCounter(&rtb_Switch, 20.0, &SOC_DW.TimeCounter_c);
    if (rtb_Switch > 20.0) {
      SOC_LowLim(rtb_Abs1, &rtb_Switch2_l, socc_pct_disChrgCCV, 0.0);
    } else {
      rtb_Switch2_l = rtb_Abs1;
    }

    SOC_Conditionalcounter(&rtb_Gain3_l, socc_Nr_judge,
      &SOC_DW.Conditionalcounter1_f);
    rtb_RelationalOperator_pu = ((rtb_Gain3_l >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_c);
    SOC_Subsystem1(rtb_RelationalOperator_pu, SOC_DW.Delay_DSTATE_l,
                   rtb_Switch2_l, &SOC_B.in1, &SOC_B.in2,
                   &SOC_PrevZCX.Subsystem2_e);
    SOC_Conditionalcounter(&Switch_cz, socc_Nr_judge,
      &SOC_DW.Conditionalcounter_n);
    SOC_DW.UnitDelay_DSTATE_c = SOC_DW.UnitDelay_DSTATE_f;
    SOC_DW.UnitDelay_DSTATE_f = ((Switch_cz >= socc_Nr_judge) ||
      SOC_DW.UnitDelay_DSTATE_c);
    if (SOC_DW.UnitDelay_DSTATE_f) {
      socd_pct_battSocEo = socc_pct_battSocLow;
    } else {
      if (rtb_RelationalOperator_pu) {
        rtb_Switch2_l = (SOC_DW.Delay_DSTATE_l - SOC_B.in1) + fminf
          (socc_pct_disChrgCCV, SOC_B.in2);
      }

      SOC_LowLim(rtb_Switch2_l, &socd_pct_battSocEo, socc_pct_battSocLow, 0.1);
    }

    SOC_Conditionalcounter_Update(ihd_V_cellUMin <= look1_iflf_binlcapw
      (rtb_OCVSOC1, &socm_I_disChrgCor[0], &socm_V_disChrgCor[0], 1U),
      &rtb_Gain3_l, &SOC_DW.Conditionalcounter1_f);
    SOC_DW.UnitDelay_DSTATE_c = rtb_RelationalOperator_pu;
    SOC_Conditionalcounter_Update(ihd_V_cellUMin <= look1_iflf_binlcapw
      (rtb_OCVSOC1, &socm_I_disChrgCor[0], &socm_V_emptyV[0], 1U), &Switch_cz,
      &SOC_DW.Conditionalcounter_n);
  }

  rtb_Abs1 = 1.0F - rtb_Add_f_tmp_tmp * rtb_MathFunction1_l[0];
  rtb_Switch2_l = 0.0F - rtb_MathFunction1_g * rtb_MathFunction1_l[0];
  tmp_0 = 1.0F - rtb_Delay_j * rtb_MathFunction1_b[0];
  tmp_1 = 0.0F - rtb_Switch7 * rtb_MathFunction1_b[0];
  for (Switch = 0; Switch < 2; Switch++) {
    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 = Switch << 1;
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp] = 0.0F;
    P_Delay_DSTATE_k_tmp_0 = rtb_Add_ij[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_ij[P_Delay_DSTATE_k_tmp + 1];
    SOC_DW.P_Delay_DSTATE_k[P_Delay_DSTATE_k_tmp] += (0.0F - rtb_Add_f_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 *
      rtb_Switch2_l;
    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_ob[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_ob[P_Delay_DSTATE_k_tmp + 1];
    SOC_DW.P_Delay_DSTATE[P_Delay_DSTATE_k_tmp] += (0.0F - rtb_Delay_j) *
      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_Switch7) *
      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) <= 100) {
    Switch = (uint16_T)(SOC_DW.UnitDelay_DSTATE_m + 1U);
  } else {
    Switch = 100;
  }

  if (SOC_DW.icLoad_m) {
    SOC_DW.Delay2_DSTATE_b = dcmd_st_chrgSt_T2;
  }

  SOC_FristEn(&rtb_RelationalOperator_pu, &SOC_DW.FristEn_n);
  if (rtb_RelationalOperator_pu || ((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) || (Switch > (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_fit_Reset(&SOC_DW.fit);
      SOC_DW.UnitDelay_DSTATE_b = 0.0F;
    }

    rtb_Delay_j = 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);
    SOC_Subsystem(socd_pct_battSocEo, socc_pct_battSocUp, fminf
                  (socc_pct_battSocUp, rtb_Delay_j),
                  SOC_DW.socd_pct_battSoc0_Delay_DSTATE, socc_pct_battSocUp,
                  SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE, &rtb_Delay_j);
    SOC_fit(rtb_Delay_j, &rtb_Switch7, socc_pct_fitRate, &SOC_DW.fit);
    if (rtb_Switch7 >= SOC_DW.UnitDelay_DSTATE_b) {
      SOC_DW.UnitDelay_DSTATE_b = rtb_Switch7;
    }

    if (SOC_DW.UnitDelay_DSTATE_e) {
      SOC_DW.Delay_DSTATE = socc_pct_battSocUp;
    } else {
      SOC_UpLim(SOC_DW.UnitDelay_DSTATE_b, &SOC_DW.Delay_DSTATE,
                socc_pct_battSocUp, -0.1);
    }
  } else {
    if (rtPrevAction != 1) {
      SOC_fit_Reset(&SOC_DW.fit_j);
    }

    rtb_Delay_j = 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));
    SOC_Subsystem(socd_pct_battSocEo, SOC_DW.socd_pct_battSoc0_Delay_DSTATE,
                  fmaxf(rtb_Delay_j, socc_pct_battSocLow), socc_pct_battSocLow,
                  SOC_DW.socd_pct_bcuSoc0_Delay_DSTATE, socc_pct_battSocLow,
                  &rtb_Switch7);
    SOC_fit(rtb_Switch7, &rtb_Delay_j, socc_pct_fitRate, &SOC_DW.fit_j);
    if (SOC_DW.UnitDelay_DSTATE_f) {
      SOC_DW.Delay_DSTATE = socc_pct_battSocLow;
    } else {
      SOC_LowLim(rtb_Delay_j, &SOC_DW.Delay_DSTATE, socc_pct_battSocLow, 0.1);
    }
  }

  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.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_Abs + rtb_Merge3;
  SOC_Conditionalcounter1_Update((!(fabsf(rtb_Abs) > 0.05F)) && (!(fabsf
    (rtb_Divide_l) > 0.05F)), &rtb_Gain3, &SOC_DW.Conditionalcounter1);
  SOC_DW.icLoad_l = false;
  SOC_DW.Delay1_DSTATE_n = socd_pct_battSocEo;
  SOC_DW.UnitDelay_DSTATE_m = (uint16_T)((fabsf(rtb_OCVSOC1) < 1.0F) * Switch);
  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;
}