#include "CDM.h"
#define cdmd_L_rls 51
void CDM_Init(void)
{
    cdmd_flg_firstRun = true;
}

void CDM(void)
{
    real_T temp_curr;
    real_T cmdn_V_cellUDelt;
    static real_T cdmn_M_P[2][2][cmnc_num_cellUNumMax];
    real_T P[2][2];
    real_T A[2];
    real_T K[2];
    real_T arf;
    real_T theta[2];
    static real_T deltaE[cmnc_num_cellUNumMax];
    static real_T deltaR[cmnc_num_cellUNumMax];
    static int16_T cdmn_I_curr[cdmd_L_rls];
    uint16_T i;
    static uint8_T cmdn_st_workStat_Delay;
    static uint8_T cdmn_num_Cnt;

    //------模式切换后重置--------------------
    if (cmdn_st_workStat_Delay != 1 && ihd_st_workStat == 1)
    {
        cdmd_flg_firstRun = true;
    }

    //初值归零---------------------------------
    if (cdmd_flg_firstRun)
    {
        cdmn_num_Cnt = 0;
        for (i = 0; i < cmnc_num_cellUNum; i++)
        {
            cdmn_M_P[0][0][i] = 10;
            cdmn_M_P[0][1][i] = 0;
            cdmn_M_P[1][0][i] = 0;
            cdmn_M_P[1][1][i] = 10;
            deltaE[i] = 0;
            deltaR[i] = 0;
            cdmv_flg_inval[i] = 1;
        }
    }
    //记忆区间-----------------------------------
    if (cdmd_flg_firstRun)
    {
        for (i = 0; i < cdmd_L_rls; i++)
        {
            cdmn_I_curr[i] = sfmd_I_curr;
        }
    }
    else
    {
        for (i = 0; i < cdmd_L_rls - 1; i++)
        {
            cdmn_I_curr[i] = cdmn_I_curr[i + 1];
        }
        cdmn_I_curr[i] = sfmd_I_curr;
    }

    //放电使能----------------------------------------
    if (cmdn_st_workStat_Delay == 1 && ihd_st_heatStat == 0)
    {
        for (i = 1; i < cmnc_num_cellUNum; i++)
        {
            //新增
            cmdn_V_cellUDelt = (real_T)(sfmv_V_cellU[i] - sfmv_V_cellU[0]) * 0.001; // #0单体为基准
            A[0] = 1;
            A[1] = (real_T)cdmn_I_curr[cdmd_L_rls - 1] * 0.1;

            theta[0] = deltaE[i];
            theta[1] = deltaR[i];
            K[0] = (cdmn_M_P[0][0][i] * A[0] + cdmn_M_P[0][1][i] * A[1]) / (1 + (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]) * A[0] + (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]) * A[1]);
            K[1] = (cdmn_M_P[1][0][i] * A[0] + cdmn_M_P[1][1][i] * A[1]) / (1 + (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]) * A[0] + (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]) * A[1]);

            arf = cmdn_V_cellUDelt - (theta[0] * A[0] + theta[1] * A[1]);
            theta[0] = theta[0] + K[0] * arf;
            theta[1] = theta[1] + K[1] * arf;

            P[0][0] = cdmn_M_P[0][0][i] - K[0] * (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]);
            P[0][1] = cdmn_M_P[0][1][i] - K[0] * (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]);
            P[1][0] = cdmn_M_P[1][0][i] - K[1] * (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]);
            P[1][1] = cdmn_M_P[1][1][i] - K[1] * (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]);

            cdmn_M_P[0][0][i] = P[0][0];
            cdmn_M_P[0][1][i] = P[0][1];
            cdmn_M_P[1][0][i] = P[1][0];
            cdmn_M_P[1][1][i] = P[1][1];

            //删去
            if (cdmn_num_Cnt > cdmd_L_rls + 1)
            {
                A[0] = 1;
                A[1] = (real_T)cdmn_I_curr[0] * 0.1;
                K[0] = (cdmn_M_P[0][0][i] * A[0] + cdmn_M_P[0][1][i] * A[1]) / (1 + (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]) * A[0] + (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]) * A[1]);
                K[1] = (cdmn_M_P[1][0][i] * A[0] + cdmn_M_P[1][1][i] * A[1]) / (1 + (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]) * A[0] + (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]) * A[1]);

                P[0][0] = cdmn_M_P[0][0][i] + K[0] * (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]);
                P[0][1] = cdmn_M_P[0][1][i] + K[0] * (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]);
                P[1][0] = cdmn_M_P[1][0][i] + K[1] * (A[0] * cdmn_M_P[0][0][i] + A[1] * cdmn_M_P[1][0][i]);
                P[1][1] = cdmn_M_P[1][1][i] + K[1] * (A[0] * cdmn_M_P[0][1][i] + A[1] * cdmn_M_P[1][1][i]);

                cdmn_M_P[0][0][i] = P[0][0];
                cdmn_M_P[0][1][i] = P[0][1];
                cdmn_M_P[1][0][i] = P[1][0];
                cdmn_M_P[1][1][i] = P[1][1];
            }

            cdmv_flg_inval[i] = (arf > 0.04 || arf < -0.04);


            deltaE[i] = theta[0];
            deltaR[i] = theta[1];
			
            cdmv_V_deltOCV[i] = (int16_T)(deltaE[i] * 1000);
            cdmv_R_deltOhm[i] = (int16_T)(deltaR[i] * 1000 * 1000);
        }
        //
        cdmn_num_Cnt = cdmn_num_Cnt + 1;
    }
    else
    {
       
        memset(cdmv_V_deltOCV, 0, sizeof(cdmv_V_deltOCV));
        memset(cdmv_R_deltOhm, 0, sizeof(cdmv_R_deltOhm));
    }

    cdmd_flg_firstRun = false;
    cmdn_st_workStat_Delay = ihd_st_workStat;
}