#include "PIM.h"

static boolean_T FirstRun_PIM;
#define pimd_L_rls 101

void PIM_Init(void)
{
    FirstRun_PIM = true;
}

void PIM(void)
{
    static uint8_T ihd_st_workStat_Delay;
    uint16_T i;
    uint16_T j;
    static uint16_T pimn_V_volt[pimd_L_rls];
    static int16_T  pimn_I_curr[pimd_L_rls];
    static real_T  pimn_V_ocv;
    static real_T  pimn_R_ohm;
    static real_T  pimn_R_polar;
    static real_T  pimn_F_polar;
    
    static uint16_T pimn_N_ctn;
    static real_T pimn_M_P[4][4];
    static real_T theta[4];
    real_T P[4][4];
    real_T A[4];
    real_T K[4];
    real_T S[4];
    real_T temp;
    //重置
    if (ihd_st_workStat_Delay != 1 && ihd_st_workStat == 1)
    {
        FirstRun_PIM = true;
    }
    
    
    //限定记忆长度
    if (FirstRun_PIM)
    {
        for (i = 0; i < pimd_L_rls; i++)
        {
            pimn_V_volt[i] = sfmv_V_cellU[0];
            pimn_I_curr[i] = sfmd_I_curr;
        }
	}
    else
    {
        for (i = 0; i < pimd_L_rls - 1; i++)
        {
            pimn_V_volt[i]  = pimn_V_volt[i+1];
            pimn_I_curr[i] = pimn_I_curr[i+1];
        }
        pimn_V_volt[i] = sfmv_V_cellU[0];
        pimn_I_curr[i] = sfmd_I_curr;
    }

    
    //初值
    if (FirstRun_PIM)
    {
        pimn_M_P[0][0] = 10; pimn_M_P[0][1] =  0;  pimn_M_P[0][2] =  0;  pimn_M_P[0][3] =  0;
        pimn_M_P[1][0] =  0; pimn_M_P[1][1] = 10;  pimn_M_P[1][2] =  0;  pimn_M_P[1][3] =  0;
        pimn_M_P[2][0] =  0; pimn_M_P[2][1] =  0;  pimn_M_P[2][2] = 10;  pimn_M_P[2][3] =  0;
        pimn_M_P[3][0] =  0; pimn_M_P[3][1] =  0;  pimn_M_P[3][2] =  0;  pimn_M_P[3][3] = 10;
        
        pimn_V_ocv   = 3.6;
        pimn_R_ohm   = 0.002;
        pimn_R_polar = 0.001;
        pimn_F_polar = 10;
        pimn_N_ctn = 0;
        
    }
    //
    if(ihd_st_workStat_Delay == 1)
    {
        pimn_N_ctn = pimn_N_ctn + 1;
        //输入
        A[0] = (real_T)pimn_V_volt[pimd_L_rls - 2] * 0.001;
        A[1] = (real_T)pimn_I_curr[pimd_L_rls - 1] * 0.1;
        A[2] =-(real_T)pimn_I_curr[pimd_L_rls - 2] * 0.1;
        A[3] = 1;
        //参数形变
        temp = exp(- (pimn_R_ohm + pimn_R_polar)/pimn_R_ohm/pimn_F_polar);
        theta[0] = 1- pimn_R_ohm * (1 - temp)/(pimn_R_ohm + pimn_R_polar);
        theta[1] = pimn_R_ohm;
        theta[2] = pimn_R_ohm * temp;
        theta[3] = (1 -theta[0]) * pimn_V_ocv;

        //增益矩阵
        temp =  (A[0] * pimn_M_P[0][0] +A[1] * pimn_M_P[1][0] + A[2] * pimn_M_P[2][0] + A[3] * pimn_M_P[3][0]) * A[0] +
                (A[0] * pimn_M_P[0][1] +A[1] * pimn_M_P[1][1] + A[2] * pimn_M_P[2][1] + A[3] * pimn_M_P[3][1]) * A[1] +
                (A[0] * pimn_M_P[0][2] +A[1] * pimn_M_P[1][2] + A[2] * pimn_M_P[2][2] + A[3] * pimn_M_P[3][2]) * A[2] +
                (A[0] * pimn_M_P[0][3] +A[1] * pimn_M_P[1][3] + A[2] * pimn_M_P[2][3] + A[3] * pimn_M_P[3][3]) * A[3];
        for(i = 0;i < 4;i++)
        {
            K[i] = (pimn_M_P[i][0] * A[0] + pimn_M_P[i][1] * A[1] + pimn_M_P[i][2] * A[2] + pimn_M_P[i][3] * A[3])/(1 + temp);
        }
        //参数更新
        temp = (real_T)pimn_V_volt[pimd_L_rls - 1] * 0.001 - (theta[0] * A[0] + theta[1] * A[1] + theta[2] * A[2] + theta[3] * A[3]);
        for(i = 0;i < 4;i++)
        {
            theta[i] = theta[i] + K[i] * temp;
        }
        //协方差更新
        for(i = 0;i < 4;i++)
        {
            for(j = 0;j < 4;j++)
            {
                P[i][j] = pimn_M_P[i][j] - K[i] * (A[0] * pimn_M_P[0][j] + A[1] * pimn_M_P[1][j] + A[2] * pimn_M_P[2][j] + A[3] * pimn_M_P[3][j]);
            }

        }
		
        //传递下循环
        for(i = 0;i < 4;i++)
        {
            for(j = 0;j < 4;j++)
            {
                pimn_M_P[i][j] = P[i][j];
            }
            
        }
        
        //=======================================================================
        if(pimn_N_ctn > pimd_L_rls)
        {
            //输入
            A[0] = (real_T)pimn_V_volt[0] * 0.001;
            A[1] = (real_T)pimn_I_curr[1] * 0.1;
            A[2] =-(real_T)pimn_I_curr[0] * 0.1;
            A[3] = 1;

            //增益矩阵
            temp =  (A[0] * pimn_M_P[0][0] +A[1] * pimn_M_P[1][0] + A[2] * pimn_M_P[2][0] + A[3] * pimn_M_P[3][0]) * A[0] +
                    (A[0] * pimn_M_P[0][1] +A[1] * pimn_M_P[1][1] + A[2] * pimn_M_P[2][1] + A[3] * pimn_M_P[3][1]) * A[1] +
                    (A[0] * pimn_M_P[0][2] +A[1] * pimn_M_P[1][2] + A[2] * pimn_M_P[2][2] + A[3] * pimn_M_P[3][2]) * A[2] +
                    (A[0] * pimn_M_P[0][3] +A[1] * pimn_M_P[1][3] + A[2] * pimn_M_P[2][3] + A[3] * pimn_M_P[3][3]) * A[3];
            for(i = 0;i <4;i++)
            {
                K[i] = (pimn_M_P[i][0] * A[0] + pimn_M_P[i][1] * A[1] + pimn_M_P[i][2] * A[2] + pimn_M_P[i][3] * A[3])/(1 + temp);
            }

            //协方差更新
            for(i = 0;i < 4;i++)
            {
                for(j = 0;j < 4;j++)
                {
                    P[i][j] = pimn_M_P[i][j] + K[i] * (A[0] * pimn_M_P[0][j] + A[1] * pimn_M_P[1][j] + A[2] * pimn_M_P[2][j] + A[3] * pimn_M_P[3][j]);
                }
            }
            //传递下一循环
            for(i = 0;i < 4;i++)
            {
                for(j = 0;j < 4;j++)
                {
                    pimn_M_P[i][j] = P[i][j];
                }
                
            }
        }
        //参数求解
        pimn_V_ocv   = theta[3]/(1 - theta[0]);
        pimn_R_ohm   = theta[1];
        pimn_R_polar = (theta[1] - theta[2])/(1 - theta[0]) - theta[1];
        pimn_F_polar = theta[2] * theta[1] > 0 ?  (pimn_R_ohm + pimn_R_polar)/pimn_R_ohm/ (-log(theta[2]/theta[1])) : (pimn_R_ohm + pimn_R_polar)/pimn_R_ohm;
        
        pimd_V_ocv   = (uint16_T)(pimn_V_ocv * 1000);
        pimd_R_ohm   = (uint16_T)(pimn_R_ohm * 1000 * 1000);
        pimd_R_polar = (uint16_T)(pimn_R_polar * 1000 * 1000);
        pimd_F_polar = (uint16_T)(pimn_F_polar * 1000);
    }
    //
    ihd_st_workStat_Delay = ihd_st_workStat;
    FirstRun_PIM = false; 
}