/*****************************************************************************
*
* MSO Corporation
*
* File              : calc_soc.c
* Compiler          : IAR C 5.10C Kickstart
* Revision          : $Revision: 1.0 $
* Date              : $Date: 2011 / 05 / 02
* Updated by        : $Author: Jongkwang Woo
*
* Support mail      : spirit0305@naver.com
*
* Target platform   : ST32F ARM support
*
* AppNote           : ST32F Application
*
* Description       : Calculation SOC app routines
*
****************************************************************************/

#define _CALC_SOC_

#include "calc_soc.h"
#include "pl455.h"
#include "device_param.h"
#include "dflash_chg_ctrl.h"
#include "ff_shell.h"
#include "asutil.h"
#include "console.h"
#include "includes.h"
#include <string.h>

#define SocFileName		"0:/rsoc.dat"
	
TCalculationSOC  CalculationSOC, disCalculationSOC;
TManageSOC       ManageSOC, disManageSOC;
TStoreSOC        StoreSOC, disStoreSOC, bakStoreSOC;

u8 LifeCycleSaveSoc = false;

u32 ChgPower = 0;
u32 DchPower = 0;

void CalculationCapacity(u8 flag, u32 current, u16 tick_ms, PCalculationSOC calc, PCurrent ptr);
void LoadStoreSOC(void);

u8 SSetSoc(u8 *text, u8 aDevice);

void SOC_Init(void)
{
	LoadStoreSOC();
}

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

void Calculation_10ms_Current(u16 tick_ms)
{
	s32 cCurr = device_value.CCurrent * 10;
	s32 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(u8 flag, u32 current, u16 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);
			
			if (StoreSOC.CalcSOC.LifeCycleCapacity > (device_param.sbs.data.Full_Charge_Capacity * 60 * 60)) 
			{
				StoreSOC.CalcSOC.LifeCycle += 1;
				StoreSOC.CalcSOC.LifeCycleCapacity = 0;
				LifeCycleSaveSoc = true;
			}
		}

		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_param.cal.current_offset.Int_Power)
						StoreSOC.CalcSOC.Capacity -= device_param.cal.current_offset.Int_Power;
					else
						StoreSOC.CalcSOC.Capacity = 0;
				}
			}
		}
		
		device_value.CycleCount = StoreSOC.CalcSOC.LifeCycle;
	}
}

void SaveSocData(u32 *data, u16 size)
{
	u32 len = ((size % 4) == 0) ? size / 4: (size / 4) + 1;
	u32 base = RTC_BKP_DR1;
	
	for (int i = 0; i < len; i++)
	{
		data[i] = RTC_ReadBackupRegister(base + i);
	}
}

void LoadSocData(u32 *data, u16 size)
{
	u32 len = ((size % 4) == 0) ? size / 4: (size / 4) + 1;
	u32 base = RTC_BKP_DR1;
	
	for (int i = 0; i < len; i++)
	{
		RTC_WriteBackupRegister(base + i, data[i]);
	}
}

#define SOC_SAVE_TIME		60000

u32 Soc_Save_Tick = SOC_SAVE_TIME;

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

void UpdateSocStatus(void)
{
	double tmp;

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

FRESULT UpdateStoreSOC(void)
{
	FRESULT rc = FR_LOCKED;
	FIL pFile;
	u32 wlen;
	
	if (ReqUseFatfs()) 
	{
		rc = f_open(&pFile, SocFileName, FA_WRITE);
		
		if (rc == FR_NO_FILE)
		{
			rc = f_open(&pFile, SocFileName, FA_CREATE_NEW | FA_WRITE);
			
			if (rc == FR_OK)
			{
				f_lseek(&pFile, 0);	// From start		
				f_write(&pFile, (char *)&StoreSOC, sizeof(TStoreSOC), &wlen);
				f_close(&pFile);
			}
		}
		else if (rc == FR_OK) 
		{
			if (pFile.obj.objsize != sizeof(TStoreSOC)) 
			{
				f_close(&pFile);
				f_unlink(SocFileName);
				
				rc = f_open(&pFile, SocFileName, FA_CREATE_NEW | FA_WRITE);
				
				if (rc == FR_OK) 
				{
					f_lseek(&pFile, 0);
					f_write(&pFile, (char *)&StoreSOC, sizeof(TStoreSOC), &wlen);
					f_close(&pFile);
				}
			} 
			else 
			{
				f_lseek(&pFile, 0);	// From start
				f_write(&pFile, &StoreSOC, sizeof(TStoreSOC), &wlen);
				f_close(&pFile);
			}
		}
		
		leaveFatfs();
	}
	return rc;
}

void SaveStoreSOC(void)
{
	FRESULT rc;
	
	if (LifeCycleSaveSoc == true) 
	{
		rc = UpdateStoreSOC();
		if (rc == FR_OK)
			LifeCycleSaveSoc = false;
	} 
	else 
	{
		if (Soc_Save_Tick == 0) 
		{
			if (memcmp(&bakStoreSOC, &StoreSOC, sizeof(TStoreSOC)) != 0) 
			{
				UpdateStoreSOC();
				memcpy(&bakStoreSOC, &StoreSOC, sizeof(TStoreSOC));
			}
			Soc_Save_Tick = SOC_SAVE_TIME;
		}
	}
}

void LoadStoreSOC(void)
{
	FRESULT rc;
	FIL pFile;
	u32 wlen;
    
	if (ReqUseFatfs()) 
	{
		rc = f_open(&pFile, SocFileName, FA_READ);

		if (rc == FR_NO_FILE) 
		{
			// Initialize SOC
			
			rc = f_open(&pFile, SocFileName, FA_CREATE_NEW | FA_WRITE);
			
			if (rc == FR_OK)
			{
				f_lseek(&pFile, 0);	// From start		
				f_write(&pFile, (char *)&StoreSOC, sizeof(TStoreSOC), &wlen);
				f_close(&pFile);
			}
		} 
		else if (rc == FR_OK) 
		{
			if (pFile.obj.objsize != sizeof(TStoreSOC)) 
			{
				f_close(&pFile);
				f_unlink(SocFileName);
				
				// Initialize SOC
				
				rc = f_open(&pFile, SocFileName, FA_CREATE_NEW | FA_WRITE);
				
				if (rc == FR_OK) 
				{
					f_lseek(&pFile, 0);
					f_write(&pFile, (char *)&StoreSOC, sizeof(TStoreSOC), &wlen);
					f_close(&pFile);
				}
			} 
			else 
			{
				f_lseek(&pFile, 0);	// From start
				f_read(&pFile, &StoreSOC, sizeof(TStoreSOC), &wlen);
				f_close(&pFile);
			}
		}
		leaveFatfs();
	}
}

void SetStoreSOC(u16 soc)
{
	if (soc <= 100)
	{
		s32 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(u16 cycle)
{
	StoreSOC.CalcSOC.LifeCycle = cycle;
	LifeCycleSaveSoc = true;
}

#ifdef CONSOLE_DEBUG

void SDisplayCurrentStatus(u8 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.bakChargeCapacity)) 
	{
		aParam = (float)StoreSOC.CalcSOC.bakChargeCapacity / 100;
		FloatNormalOutIntXY(50 + 14, 13 , aParam / 60 / 60,  10, 3);
		disStoreSOC.CalcSOC.bakChargeCapacity = StoreSOC.CalcSOC.bakChargeCapacity;
	}
	if (p || (disStoreSOC.CalcSOC.bakDischargeCapacity != StoreSOC.CalcSOC.bakDischargeCapacity)) 
	{
		aParam = (float)StoreSOC.CalcSOC.DischargeCapacity / 100;
		FloatNormalOutIntXY(50 + 14, 14 , aParam / 60 / 60,  10, 3);
		disStoreSOC.CalcSOC.bakDischargeCapacity = StoreSOC.CalcSOC.bakDischargeCapacity;
	}
}

/* Command processing in SETTING STATE */
s8 Process_DataCcModeSet(u8 *text, u16 size)
{
	u16 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':     /* Switch to operating mode */
		screenmode = OP_MODE;
		ViewScreen();
		break;
case 's':     /* Switch to setting status */
		screenmode = SET_MODE;
		ViewScreen();
		break;
case 'd':     /* Switch to diagnostic mode */
		screenmode = DIAG_MODE;
		ViewScreen();
		break;
	case '9':		// OCV Alarm Parameter Setting
	case 'I':		// OCV Alarm Parameter 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);
}

u8 SSetSoc(u8 *text, u8 aDevice)
{
	u16 size, i, j;
	s32 aParam[4];
	u8 temp[10];
	u8 ret;

	ret = 0;
	size = 0;

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

	switch (aDevice) 
	{
		case '9' :			// Cell Balance Parameter Setting
			i = 0;
			if (size < 1 ) return(1);
					
			j = FIND_CHAR((u8 *)&text[i], temp, size - i, ' ');            
			i = i + j + 1;                                                    
			temp[j] = 0x00;
			aParam[0] = StrToInt(temp, j);                                      /* IP a */

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

#endif // #ifdef CONSOLE_DEBUG