#include "SOR.h"


boolean_T FirstRun_SOR;

void SOR_Init(void)
{
    FirstRun_SOR = true;
}
//-------------------------------------------------------------------------
void SOR(void)
{
    static uint16_T sorn_ohm_cellREE[cmnc_num_cellUNumMax];
    static uint16_T sorn_ohm_cellREEArr[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;
	socd_pct_battSoc = 600;
    //
    if(FirstRun_SOR)
    {
        sorn_num_ctn = 0;
        sorn_I_curr = sfmd_I_curr;
        memcpy(sorn_V_cellU,sfmv_V_cellU, sizeof(sfmv_V_cellU));
    }
    if(FirstRun_SOR)
    {
        if(ArrMax((&(sorv_ohm_cellREi[0])) , cmnc_num_cellUNum) > 65000)
        {
            memset(sorn_ohm_cellREE,0, sizeof(sorn_ohm_cellREE));
        }
        else
        {
            memcpy(sorn_ohm_cellREE,sorv_ohm_cellREi, sizeof(sorv_ohm_cellREi));
        }
    }

    
    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_ohm_cellREEArr[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_ohm_cellREEArr[j][i];
                }
                sorn_ohm_cellREE[i] = sorn_ohm_cellRSum[i]/sorn_num_ctn;
            }
        } 
    }
    memcpy(sorv_ohm_cellREo,sorn_ohm_cellREE, sizeof(sorn_ohm_cellREE));
	sorn_I_curr = sfmd_I_curr;
    memcpy(sorn_V_cellU,sfmv_V_cellU, sizeof(sfmv_V_cellU));
    FirstRun_SOR = false;
}