#include "SOR.h"

void SOR_Init(void)
{
    sord_flg_firstRun = true;
}
//-------------------------------------------------------------------------
void SOR(void)
{
    static uint16_T sorn_R_cellEE[cmnc_num_cellUNumMax];
    static uint16_T sorn_R_cellEEArr[5][cmnc_num_cellUNumMax];
    uint16_T sorn_ohm_cellRSum[cmnc_num_cellUNumMax];
	static uint16_T sorn_V_cellU[cmnc_num_cellUNumMax];
	static uint8_T sorn_num_ctn;
	static int16_T sorn_I_curr;
    uint16_T i;
    uint16_T j;
	
    //
    if(sord_flg_firstRun)
    {
        sorn_num_ctn = 0;
        sorn_I_curr = sfmd_I_curr;
        memcpy(sorn_V_cellU,sfmv_V_cellU, sizeof(sfmv_V_cellU));
    }
    if(sord_flg_firstRun)
    {
        if(ArrMax(sorv_R_cellEi, cmnc_num_cellUNum) > 65000)
        {
            memset(sorn_R_cellEE,0, sizeof(sorn_R_cellEE));
        }
        else
        {
            memcpy(sorn_R_cellEE,sorv_R_cellEi, sizeof(sorv_R_cellEi));
        }
    }

    
    if(socd_pct_battSoc >= 550 && socd_pct_battSoc <= 700 && sorn_num_ctn < 5)
    {
        if(sfmd_I_curr - sorn_I_curr > 200 || sfmd_I_curr - sorn_I_curr < -200)
        {
            for(i = 0;i < cmnc_num_cellUNum;i++)
            {
                sorn_R_cellEEArr[sorn_num_ctn][i] = (uint16_T)( ((real_T)(sorn_V_cellU[i] - sfmv_V_cellU[i]) * 0.001) / ((real_T)(sorn_I_curr - sfmd_I_curr) * 0.1) * 1000000 );
            }
            sorn_num_ctn = sorn_num_ctn + 1;
            memset(sorn_ohm_cellRSum,0, sizeof(sorn_ohm_cellRSum));
            for(i = 0;i < cmnc_num_cellUNum;i++)
            {
                for(j = 0;j < sorn_num_ctn;j++)
                {
                    sorn_ohm_cellRSum[i] =  sorn_ohm_cellRSum[i] + sorn_R_cellEEArr[j][i];
                }
                sorn_R_cellEE[i] = sorn_ohm_cellRSum[i]/sorn_num_ctn;
            }
        } 
    }
	memcpy(sorv_R_cell,sorn_R_cellEE, sizeof(sorn_R_cellEE));
    memcpy(sorv_R_cellEo,sorn_R_cellEE, sizeof(sorn_R_cellEE));
	
	sorn_I_curr = sfmd_I_curr;
    memcpy(sorn_V_cellU,sfmv_V_cellU, sizeof(sfmv_V_cellU));
    sord_flg_firstRun = false;
}