#include "SOP.h"

void SOP_Init(void)
{
    sopd_flg_firstRun = true;
}
//-------------------------------------------------------------------------

void SOP(void)
{
    static real_T sopn_V_up;
    real_T sopn_I_currPred;
    real_T sopn_V_battPred10;
    real_T sopn_pct_socPred10;
    real_T ocv;
    real_T Ro;
    real_T RC;
    real_T Rp;
    real_T ileft;
    real_T iright;
    real_T imid;
    real_T sopn_V_battU;
    if(sopd_flg_firstRun)
    {
        sopn_V_up = 0;
    }
    ocv = (real_T)look1_u16tu16(socd_pct_battSoc, cmnm_pct_soc, cmnm_V_ocv,  13) * 0.001;
    Ro  = (real_T)look1_u16tu16(socd_pct_battSoc, cmnm_pct_soc, cmnm_R_ohm,  13) * 0.001 * 0.001;
    Rp  = (real_T)look1_u16tu16(socd_pct_battSoc, cmnm_pct_soc, cmnm_R_polar,13) * 0.001 * 0.001;
    RC  = (real_T)look1_u16tu16(socd_pct_battSoc, cmnm_pct_soc, cmnm_F_polar,13) * 0.001;
	//printf("%f %f  %f  %f\n",ocv,Ro,Rp,RC);
    sopn_V_up = (real_T)sfmd_V_cellUAvrg * 0.001 - ocv - (real_T)sfmd_I_curr * 0.1 * Ro;
    //printf("soc:%d,I:%d,up:%f\n",socd_pct_battSoc,sfmd_I_curr,sopn_V_up);
    //充电功率
    sopn_I_currPred = 0;
    predSystem(ocv,Ro,Rp,RC,sopn_I_currPred,sopn_V_up,&sopn_pct_socPred10,&sopn_V_battPred10);

    if(sopn_V_battPred10 > (real_T)cmnc_V_chrgFul * 0.001 || sopn_pct_socPred10 > (real_T)socc_pct_battSocUp * 0.1)
    {
        sopn_I_currPred = 0;
        
    }
    else
    {
        sopn_I_currPred = (real_T)sopc_I_currUp * 0.1;
        predSystem(ocv,Ro,Rp,RC,sopn_I_currPred,sopn_V_up,&sopn_pct_socPred10,&sopn_V_battPred10);
        if(sopn_V_battPred10 < (real_T)cmnc_V_chrgFul * 0.001 && sopn_pct_socPred10 < (real_T)socc_pct_battSocUp * 0.1)
        {
            sopn_I_currPred = (real_T)sopc_I_currUp * 0.1;;
        }
        else
        {
            iright = (real_T) sopc_I_currUp * 0.1;
            ileft = 0;
            imid = (iright + ileft)/2;
            while(iright - ileft > 0.1)
            {
                predSystem(ocv,Ro,Rp,RC,imid,sopn_V_up,&sopn_pct_socPred10,&sopn_V_battPred10);
                
                if(sopn_V_battPred10 > (real_T)cmnc_V_chrgFul * 0.001 || sopn_pct_socPred10 > (real_T)socc_pct_battSocUp * 0.1)
                {
                    iright =  imid;
                }
                else
                {
                    ileft =  imid;
                }
                imid = (iright + ileft)/2;
            }
			sopn_I_currPred = ileft;
        }
    }
    sopn_V_battU = (ocv +  sopn_I_currPred * Ro + sopn_V_up) * cmnc_num_cellUNum;
	sopd_I_chrgCurrMax = (int16_T)(sopn_I_currPred * 10);
    sopd_P_chrgPMax = (uint16_T)(sopn_V_battU * sopn_I_currPred);
	predSystem(ocv,Ro,Rp,RC,sopn_I_currPred,sopn_V_up,&sopn_pct_socPred10,&sopn_V_battPred10);
    //printf("%f,  %f, %f\n\n",sopn_I_currPred,sopn_pct_socPred10,sopn_V_battPred10);

	
    //放电功率
    
    sopn_I_currPred = 0;
    predSystem(ocv,Ro,Rp,RC,sopn_I_currPred,sopn_V_up,&sopn_pct_socPred10,&sopn_V_battPred10);
    if(sopn_V_battPred10 < (real_T)cmnc_V_disChrgLim * 0.001 || sopn_pct_socPred10 < (real_T)socc_pct_battSocLow * 0.1)
    {
        sopn_I_currPred = 0;
    }
    else
    {
        sopn_I_currPred = (real_T)sopc_I_currLow * 0.1;
        predSystem(ocv,Ro,Rp,RC,sopn_I_currPred,sopn_V_up,&sopn_pct_socPred10,&sopn_V_battPred10);
        if(sopn_V_battPred10 > (real_T)cmnc_V_disChrgLim * 0.001 && sopn_pct_socPred10 > (real_T)socc_pct_battSocLow * 0.1)
        {
            sopn_I_currPred = (real_T)sopc_I_currLow * 0.1;;
        }
        else
        {
            iright = 0;
            ileft = (real_T) sopc_I_currLow * 0.1;
            imid = (iright + ileft)/2;
            while(iright - ileft > 0.1)
            {
                predSystem(ocv,Ro,Rp,RC,imid,sopn_V_up,&sopn_pct_socPred10,&sopn_V_battPred10);
                if(sopn_V_battPred10 > (real_T)cmnc_V_disChrgLim * 0.001 && sopn_pct_socPred10 > (real_T)socc_pct_battSocLow * 0.1)
                {
                    iright =  imid;
                }
                else
                {
                    ileft =  imid;
                }
                imid = (iright + ileft)/2;
            }
            sopn_I_currPred = iright;
        }
    }
    sopn_V_battU = sopn_V_battPred10 * cmnc_num_cellUNum;
	sopd_I_disChrgCurrMax = (int16_T)(sopn_I_currPred * 10);
    sopd_P_disChrgPMax = (uint16_T)(- sopn_V_battU * sopn_I_currPred);
    
    
    //sopn_V_up = sopn_V_up * exp(-1/RC) + Rp * (1 - exp(-1/RC)) * sfmd_I_curr;
    sopd_flg_firstRun = false;
}

void predSystem(real_T ocv,real_T Ro,real_T Rp,real_T RC,real_T sopn_I_currPred,real_T sopn_V_up,real_T *sopn_pct_socPred10,real_T *sopn_V_battPred10)
{
    uint8_T i;
    real_T sopn_V_upPred;
    real_T sopn_pct_socPred;
    real_T sopn_Q_cellCap;
    sopn_Q_cellCap = (real_T)sohd_pct_bcuSoh * cmnc_Q_ratedCp * 0.0001;
    sopn_pct_socPred = (real_T) socd_pct_battSoc * 0.1;
    sopn_V_upPred = sopn_V_up;
    for(i = 0;i < 10;i++)
    {
        sopn_V_upPred = sopn_V_upPred * exp(-1/RC) + Rp * (1 - exp(-1/RC)) * sopn_I_currPred;
        sopn_pct_socPred = sopn_pct_socPred + sopn_I_currPred/sopn_Q_cellCap/36;
    }
    *sopn_pct_socPred10 = sopn_pct_socPred;
    *sopn_V_battPred10 = ocv +  sopn_I_currPred * Ro + sopn_V_upPred;
}