/**
  ******************************************************************************
  * @file    calc_soc.c
  * @author  AMG Energy Solution Team
  * @version V1.0.0
  * @date    07-Feb-2022
  * @brief   This file provides all the codes of calculation SOC functions.
  ******************************************************************************
  * @attention
  *
  * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
  * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
  * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
  * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
  * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
  * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
  *
  * <h2><center>&copy; COPYRIGHT 2022 AMG</center></h2>
  ******************************************************************************
  */ 

/** @addtogroup mBMS2
  * @{
  */

/* Includes ------------------------------------------------------------------*/
#include <includes.h>
#include <string.h>
#include "calc_soc.h"
#include "rtc_io.h"
#include "device_param.h"
#include "dflash_chg_ctrl.h"
#include "asutil.h"
#include "console.h"
#include "io.h"
#include "flash_if.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define CYCLE_LIFE															(double)2300.0

#define PWR_CONSUMPTION_BMS                			100         // 4W 
#define PWR_CONSUMPTION_LIFE                		300         // 13W (250mA) 

#define CHARGE_VOLTAGE_SOC                      900         // 10.0%
#define DICHARGE_VOLTAGE_SOC                    100         // 10.0%

#define SocFileName				"0:/rsoc.dat"

#define SOC_NV_RAM_BASE		0x02				// 0x00 ~ 0x01: Init value
																			// 0x04 ~ 

#define SOC_INIT_DATA          	0xCCCC0004

/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
TCalculationSOC  CalculationSOC, disCalculationSOC;
TManageSOC       ManageSOC, disManageSOC;
TStoreSOC        StoreSOC, disStoreSOC, bakStoreSOC;

uint8_t LifeCycleSaveSoc = false;

uint32_t ChgPower = 0;
uint32_t DchPower = 0;

/* Private function prototypes -----------------------------------------------*/
void CalculationCapacity(uint8_t flag, uint32_t current, uint16_t tick_ms, PCalculationSOC calc, PCurrent ptr);

/* Private functions ---------------------------------------------------------*/

uint8_t SSetSoc(uint8_t *text, uint8_t aDevice);

void SOC_Init(void)
{
	LoadStoreSOC();
}

void ProcessSOC(void)
{
	UpdateSocStatus();
	if (device_status.battery_status.bit.INIT) {
		TapperCheck();
		FullDischargeCheck();
	}
}

void Calculation_10ms_Current(uint16_t tick_ms)
{
	int32_t cCurr = device_value.CCurrent * 10;
	int32_t dCurr = device_value.DCurrent * 10;

	if (device_status.battery_status.bit.INIT) {
		if (dCurr >= cCurr) {
			// Discharge Mode
			if (dCurr < 50)	dCurr = 0;
			CalculationCapacity(1, dCurr, tick_ms, &CalculationSOC, &ManageSOC.Discharge);
		} else {
			// Charge Mode
			if (cCurr < 50)	cCurr = 0;
			CalculationCapacity(0, cCurr, tick_ms, &CalculationSOC, &ManageSOC.Charge);
		}
	}
}

void CalculationCapacity(uint8_t flag, uint32_t current, uint16_t tick_ms, PCalculationSOC calc, PCurrent ptr)
{
	calc->CountPer10msec += tick_ms;
	calc->CurrPer10msec = current;
	
	ptr->CurrPerSecBuf += current;	// 100mA
    
	if (calc->CountPer10msec >= 1000) {
		ptr->CurrPerSec = (ptr->CurrPerSecBuf / (1000 / tick_ms));
		ptr->CurrPerMinBuf += ptr->CurrPerSec;

		ptr->CurrPerHourBuf += current;
        
		if (flag == 0) {
			// Charge Calculation 
			StoreSOC.CalcSOC.Capacity += (ptr->CurrPerSec / 1);
			StoreSOC.CalcSOC.ChargeCapacity += (ptr->CurrPerSec / 1);
			ChgPower += (ptr->CurrPerSec * device_value.BatVoltage)/ 1000;
			if (ptr->CurrPerSec > 100) {
				StoreSOC.CalcSOC.DischargeCapacity = 0;
				DchPower = 0;
			}
			if (StoreSOC.CalcSOC.Capacity > (device_param.sbs.data.Full_Charge_Capacity * 60 * 60))
				StoreSOC.CalcSOC.Capacity = (device_param.sbs.data.Full_Charge_Capacity * 60 * 60);
		} else {
			// Discharge Calculation 
			if (StoreSOC.CalcSOC.Capacity < (ptr->CurrPerSec / 1)) {
				StoreSOC.CalcSOC.Capacity = 0;
			} else {
				if (device_value.Cell.avg.avg < 3450) 
					StoreSOC.CalcSOC.Capacity -= (ptr->CurrPerSec / 1);
			}
			DchPower += (ptr->CurrPerSec * device_value.BatVoltage)/ 1000;
			StoreSOC.CalcSOC.DischargeCapacity += (ptr->CurrPerSec / 1);
			StoreSOC.CalcSOC.LifeCycleCapacity += (ptr->CurrPerSec / 1);
		}

		ptr->CurrPerSecBuf = 0;
		
		calc->CountPer10msec = 0;
		calc->CountPer1min++;
        
		if (calc->CountPer1min >= 60) {
			ptr->CurrPerMin = (ptr->CurrPerMinBuf / 60);
			ptr->CurrPerMinBuf = 0;
			calc->CountPer1min = 0;

			calc->CountPer1hour++;
            
			if (calc->CountPer1hour >= 60) {
				ptr->CurrPerHour = (ptr->CurrPerHourBuf / (60 * 60));
				ptr->CurrPerHourBuf = 0;
				
				calc->CountPer1hour = 0;
				
				if (device_value.Cell.avg.avg < 3450) {
					if (StoreSOC.CalcSOC.Capacity > device_calib_data.cal.current_offset.Int_Power)
						StoreSOC.CalcSOC.Capacity -= device_calib_data.cal.current_offset.Int_Power;
					else
						StoreSOC.CalcSOC.Capacity = 0;
				}
			}
		}
		
		// Life Time Calculation
		if (device_value.RelativeStateOfCharge <= 500) {
			// Power consumption for Life
			StoreSOC.CalcSOC.LifeCycleCapacity += device_calib_data.cal.current_offset.Int_Power;
		} else if (device_value.RelativeStateOfCharge > 900) {
			// Power consumption for Life
			StoreSOC.CalcSOC.LifeCycleCapacity += device_calib_data.cal.current_offset.LifePowerCons;
		} else {
			int a = device_calib_data.cal.current_offset.LifePowerCons - device_calib_data.cal.current_offset.Int_Power;
			int b = device_value.RelativeStateOfCharge - 500;
			if (a > 0) {
				int c = a * b / 500 + device_calib_data.cal.current_offset.Int_Power; // 200 * 400 / 500 = 80000 / 500 = 160
				StoreSOC.CalcSOC.LifeCycleCapacity += c;
			}
		}

		if (StoreSOC.CalcSOC.LifeCycleCapacity > (device_param.sbs.data.Design_Capacity * 60 * 60)) {
			StoreSOC.CalcSOC.LifeCycle += 1;
			StoreSOC.CalcSOC.LifeCycleCapacity = 0;
			LifeCycleSaveSoc = true;
		}		
		device_value.CycleCount = StoreSOC.CalcSOC.LifeCycle;
	}
}

void LoadSocData(uint8_t *data, uint16_t size)
{
	uint32_t base = SOC_NV_RAM_BASE;
	
	for (int i = 0; i < size; i++) {
		data[i] = rtc_read_nvr(base + i);
	}
}

void SaveSocData(uint8_t *data, uint16_t size)
{
	uint32_t base = SOC_NV_RAM_BASE;
	
	for (int i = 0; i < size; i++) {
		rtc_write_nvr(base + i, data[i]);
	}
}

#define SOC_SAVE_TIME		(10UL * 60UL * 1000UL)	// 10 minutes (600,000 ms)

uint32_t Soc_Save_Tick = SOC_SAVE_TIME;

void dec_soc_tick(uint32_t tick)
{
	if (Soc_Save_Tick > tick) Soc_Save_Tick -= tick;
	else											Soc_Save_Tick = 0;
}

void UpdateSocStatus(void)
{
	double tmp;

	device_value.RemainingCapacity = (uint16_t)((float)StoreSOC.CalcSOC.Capacity / 60 / 60);
	device_value.RelativeStateOfCharge = (uint16_t)((float)device_value.RemainingCapacity / (float)device_param.sbs.data.Full_Charge_Capacity * 1000);
	tmp = CYCLE_LIFE - (double)device_value.CycleCount;
	tmp = tmp / CYCLE_LIFE * 20;
	tmp += 80;
	device_value.StateOfHealth = (uint16_t)(tmp * 10);
}

uint32_t UpdateStoreSOC(void)
{
	uint32_t result = 0;
	uint32_t sector_addr = mBMS_SOC_BASE_ADDR;
	
	result = FLASH_If_SingleSectorErase(sector_addr);
	
	if (result == 0) {
		uint32_t base = (uint32_t)&StoreSOC;
		uint32_t len = sizeof(TStoreSOC);
		
		if ((len % 4) > 0) {
			len = len / 4 + 1;
		} else {
			len = len / 4;
		}		
		result =  FLASH_If_Write(&sector_addr, (uint32_t *)base, len);

#ifdef CONSOLE_DEBUG
		if (result != 0) {
			printf("\r\n Param save fail (%d)", result);
		}
#endif
	}
	return result;
}

void SaveStoreSOC(void)
{
	int rc;
	
	if (LifeCycleSaveSoc == true) {
		rc = UpdateStoreSOC();
		if (rc == 0) {
			LifeCycleSaveSoc = false;
			memcpy(&bakStoreSOC, &StoreSOC, sizeof(TStoreSOC));
		}
	} else {
		if (Soc_Save_Tick == 0) {
			int32_t cap_delta = (int32_t)StoreSOC.CalcSOC.Capacity - (int32_t)bakStoreSOC.CalcSOC.Capacity;
			if (cap_delta < 0) cap_delta = -cap_delta;
			
			// 1% threshold of Full Charge Capacity to prevent unnecessary flash wear
			uint32_t one_percent_cap = (uint32_t)device_param.sbs.data.Full_Charge_Capacity * 36;
			if (one_percent_cap == 0) one_percent_cap = 3600;

			if ((bakStoreSOC.CalcSOC.LifeCycle != StoreSOC.CalcSOC.LifeCycle) ||
				((uint32_t)cap_delta >= one_percent_cap)) {
				UpdateStoreSOC();
				memcpy(&bakStoreSOC, &StoreSOC, sizeof(TStoreSOC));
			}
			Soc_Save_Tick = SOC_SAVE_TIME;
		}
	}
}

void LoadStoreSOC(void)
{
	memcpy(&StoreSOC, (uint8_t *)mBMS_SOC_BASE_ADDR, sizeof(TStoreSOC));
	
	if (StoreSOC.initdata != SOC_INIT_DATA) {
		// SOC init
		memset(&StoreSOC, 0x00, sizeof(TStoreSOC));
		StoreSOC.initdata = SOC_INIT_DATA;
		
		SaveStoreSOC();
	}
}

void SetStoreSOC(uint16_t soc)
{
	if (soc <= 100) {
		int32_t tmp;
		
		tmp = device_param.sbs.data.Full_Charge_Capacity;
		tmp = tmp * 60 * 60;
		tmp = (tmp / 100) * soc;
		
		StoreSOC.CalcSOC.Capacity = tmp;
		StoreSOC.CalcSOC.DischargeCapacity = 0;
		LifeCycleSaveSoc = true;

		SaveStoreSOC();
	}
}

void InitLifeCycleCount(uint16_t cycle)
{
	StoreSOC.CalcSOC.LifeCycle = cycle;
	LifeCycleSaveSoc = true;
}

#ifdef CONSOLE_DEBUG

void SDisplayCurrentStatus(uint8_t p)
{
	float aParam;
    
	if (p || (disCalculationSOC.CurrPer10msec != CalculationSOC.CurrPer10msec)) {
		aParam = (float)CalculationSOC.CurrPer10msec / 100;
		FloatNormalOutIntXY(50, 6 , aParam, 10, 2);
		disCalculationSOC.CurrPer10msec = CalculationSOC.CurrPer10msec;
	}
	if (p || (disCalculationSOC.CountPer1min != CalculationSOC.CountPer1min)) {
		aParam = (float)CalculationSOC.CountPer1min;
		FloatNormalOutIntXY(77, 8 , aParam,  2, 0);
		disCalculationSOC.CountPer1min = CalculationSOC.CountPer1min;
	}
    
	// Charge Status 
	if (p || (disManageSOC.Charge.CurrPerSec != ManageSOC.Charge.CurrPerSec)) {
		aParam = (float)ManageSOC.Charge.CurrPerSec / 100;
		FloatNormalOutIntXY(50, 7 , aParam, 10, 2);
		disManageSOC.Charge.CurrPerSec = ManageSOC.Charge.CurrPerSec;
	}
	if (p || (disManageSOC.Charge.CurrPerMin != ManageSOC.Charge.CurrPerMin)) {
		aParam = (float)ManageSOC.Charge.CurrPerMin / 100;
		FloatNormalOutIntXY(50, 8 , aParam, 10, 2);
		disManageSOC.Charge.CurrPerMin = ManageSOC.Charge.CurrPerMin;
	}
	if (p || (disManageSOC.Charge.CurrPerHourBuf != ManageSOC.Charge.CurrPerHourBuf)) {
		aParam = (float)ManageSOC.Charge.CurrPerHourBuf / 100;
		FloatNormalOutIntXY(50, 9 , aParam / 60 / 60,  10, 2);
		disManageSOC.Charge.CurrPerHourBuf = ManageSOC.Charge.CurrPerHourBuf;
	}
	// Discharge Status 
	if (p || (disManageSOC.Discharge.CurrPerSec != ManageSOC.Discharge.CurrPerSec)) {
		aParam = (float)ManageSOC.Discharge.CurrPerSec / 100;
		FloatNormalOutIntXY(50 + 14, 7 , aParam, 10, 2);
		disManageSOC.Discharge.CurrPerSec = ManageSOC.Discharge.CurrPerSec;
	}
	if (p || (disManageSOC.Discharge.CurrPerMin != ManageSOC.Discharge.CurrPerMin)) {
		aParam = (float)ManageSOC.Discharge.CurrPerMin / 100;
		FloatNormalOutIntXY(50 + 14, 8 , aParam, 10, 2);
		disManageSOC.Discharge.CurrPerMin = ManageSOC.Discharge.CurrPerMin;
	}
	if (p || (disManageSOC.Discharge.CurrPerHourBuf != ManageSOC.Discharge.CurrPerHourBuf)) {
		aParam = (float)ManageSOC.Discharge.CurrPerHourBuf / 100;
		FloatNormalOutIntXY(50 + 14, 9 , aParam / 60 / 60,  10, 2);
		disManageSOC.Discharge.CurrPerHourBuf = ManageSOC.Discharge.CurrPerHourBuf;
	}
	
	/* Capacity Display */
	if (p || (disStoreSOC.CalcSOC.Capacity != StoreSOC.CalcSOC.Capacity)) {
		aParam = (float)StoreSOC.CalcSOC.Capacity / 100;
		FloatNormalOutIntXY(50 + 14, 10 , aParam / 60 / 60,  10, 3);
		disStoreSOC.CalcSOC.Capacity = StoreSOC.CalcSOC.Capacity;
	}
	if (p || (disStoreSOC.CalcSOC.ChargeCapacity != StoreSOC.CalcSOC.ChargeCapacity)) {
		aParam = (float)StoreSOC.CalcSOC.ChargeCapacity / 100;
		FloatNormalOutIntXY(50 + 14, 11 , aParam / 60 / 60,  10, 3);
		disStoreSOC.CalcSOC.ChargeCapacity = StoreSOC.CalcSOC.ChargeCapacity;
	}
	if (p || (disStoreSOC.CalcSOC.DischargeCapacity != StoreSOC.CalcSOC.DischargeCapacity)) {
		aParam = (float)StoreSOC.CalcSOC.DischargeCapacity / 100;
		FloatNormalOutIntXY(50 + 14, 12 , aParam / 60 / 60,  10, 3);
		disStoreSOC.CalcSOC.DischargeCapacity = StoreSOC.CalcSOC.DischargeCapacity;
	}
//	/* Backup Capacity Display */
//	if (p || (disStoreSOC.CalcSOC.bakChargeCapacity != StoreSOC.CalcSOC.ChargeCapacity)) 
//	{
//		aParam = (float)StoreSOC.CalcSOC.ChargeCapacity / 100;
//		FloatNormalOutIntXY(50 + 14, 13 , aParam / 60 / 60,  10, 3);
//		disStoreSOC.CalcSOC.bakChargeCapacity = StoreSOC.CalcSOC.ChargeCapacity;
//	}
//	if (p || (disStoreSOC.CalcSOC.bakDischargeCapacity != StoreSOC.CalcSOC.DischargeCapacity)) 
//	{
//		aParam = (float)StoreSOC.CalcSOC.DischargeCapacity / 100;
//		FloatNormalOutIntXY(50 + 14, 14 , aParam / 60 / 60,  10, 3);
//		disStoreSOC.CalcSOC.bakDischargeCapacity = StoreSOC.CalcSOC.DischargeCapacity;
//	}
	
	if (p || (disStoreSOC.CalcSOC.LifeCycleCapacity != StoreSOC.CalcSOC.LifeCycleCapacity)) {
		aParam = (float)StoreSOC.CalcSOC.LifeCycleCapacity / 100;
		FloatNormalOutIntXY(50 + 14, 15 , aParam / 60 / 60,  10, 3);
		disStoreSOC.CalcSOC.LifeCycleCapacity = StoreSOC.CalcSOC.LifeCycleCapacity;
	}
	if (p || (disStoreSOC.CalcSOC.LifeCycle != StoreSOC.CalcSOC.LifeCycle)) {
		aParam = (float)StoreSOC.CalcSOC.LifeCycle / 1;
		FloatNormalOutIntXY(50 + 14, 16 , aParam,  10, 0);
		disStoreSOC.CalcSOC.LifeCycle = StoreSOC.CalcSOC.LifeCycle;
	}
}

/* Command processing in SETTING STATE */
int8_t Process_DataCcModeSet(uint8_t *text, uint16_t size)
{
	uint16_t i, ret;

	/* Invalid command if space is in first position */
	if (size > 1) {if (text[1] != ' ') return(0);}
	text[size] = 0x00;
	ret = 1;

	switch (text[0]) {
		case 'o':     /* operating mode */
			screenmode = OP_MODE;
			ViewScreen();
			break;
		case 's':     /* diagnostic mode */
			screenmode = SET_MODE;
			ViewScreen();
			break;
		case 'd':     /* diagnostic mode */
			screenmode = DIAG_MODE;
			ViewScreen();
			break;
		case '9':		// SOC Value Parameter Setting
		case 'R':		// Life Time Capacity Setting
		case 'I':		// Cycle Count Setting
			i = SSetSoc(&text[2],text[0]);
			if (i == 0)      UserMessage("COMMAND COMPLETE");
			else if (i == 1) UserMessage("COMMAND FORMAT ERROR");   /* setting format error */
			else 						 UserMessage("COMMAND LIMIT ERROR");    /* setting limit error */
			break;
		default :
			ret = 0;
			break;
	}
	return(ret);
}

uint8_t SSetSoc(uint8_t *text, uint8_t aDevice)
{
	uint16_t size, i, j;
	int32_t aParam[4];
	uint8_t temp[10];
	uint8_t ret;

	ret = 0;
	size = 0;

	while (text[size] != 0x00) size++;              /* Measure total string length */

	switch (aDevice) {
		case '9' :			// SOC Value change command
			i = 0;
			if (size < 1 ) return(1);
					
			j = FIND_CHAR((uint8_t *)&text[i], temp, size - i, ' ');            
			i = i + j + 1;                                                    
			temp[j] = 0x00;
			aParam[0] = StrToInt(temp, j);

			SetStoreSOC(aParam[0]);
			UpdateSocStatus();
			SOC_LED_Buf_Clear();
			break;
		case 'R':
			i = 0;
			if (size < 1 ) return(1);
					
			j = FIND_CHAR((uint8_t *)&text[i], temp, size - i, ' ');            
			i = i + j + 1;                                                    
			temp[j] = 0x00;
			aParam[0] = StrToInt(temp, j);
			
			StoreSOC.CalcSOC.LifeCycleCapacity = aParam[0];
			LifeCycleSaveSoc = true;
			break;
		case 'I':
			i = 0;
			if (size < 1 ) return(1);

			j = FIND_CHAR((uint8_t *)&text[i], temp, size - i, ' ');
			i = i + j + 1;
			temp[j] = 0x00;
			aParam[0] = StrToInt(temp, j);
		
			InitLifeCycleCount(aParam[0]);
			break;
		default:
			ret = 1;
			break;
	}
return(ret);                /* Setting value OK */
}

#endif // #ifdef CONSOLE_DEBUG

/*******************(C)COPYRIGHT 2022 AMG ********************END OF FILE******/