/******************************************************************************
*
* @file    ah_counter.c
* @author  ECS, Falko Jahn
* @version V1.0.0
* @date    2020-05-01
* @brief
*
******************************************************************************/

//	--- INCLUDES -----------------------------------------------------------------
#include "main.h"
#include "math.h"
#include "sysdata.h"
#include "ah_counter.h"
#include "wh_counter.h"
#include "eeprom.h"
#include "stdio.h"
//	--- EXTERNE VARIABLEN --------------------------------------------------------

//	--- LOKALE DEFINES - bitte hier dokumentieren --------------------------------

//	--- LOKALE TYPE DEFS - bitte hier dokumentieren-------------------------------

//	--- DEFINITIONEN GLOBALER VARIABLEN - Bitte in Header dokumentieren ----------

//	--- LOKALE VARIABLEN - bitte hier dokumentieren ------------------------------
int startMeasurementDischarge = 0;
int startMeasurementCEF = 0;

//	--- LOKALE FUNKTIONS PROTOTYPEN ----------------------------------------------
int getSocAhRated(void);
int getSocAhAuto(void);

//int64_t mAs_AutoMode;


void AH_COUNTER_Init(void)
{
  sys_data.s.values.mAs_AutoMode =   (int32_t)-sys_data.s.parameter.cellCapacity * 3600;
  sys_data.s.values.mAh_AutoMode =   (int32_t)-sys_data.s.parameter.cellCapacity ;

  sys_data.s.values.mAs_AutoModeU =   (int32_t)-sys_data.s.parameter.cellCapacity * 3600;
  sys_data.s.values.mAh_AutoModeU =   (int32_t)-sys_data.s.parameter.cellCapacity ;
}

void AH_COUNTER_SetDetectedAh(void)
{
  sys_data.s.values.detectedCapacity = sys_data.s.values.mAh_AutoMode >= 0 ? sys_data.s.values.mAh_AutoMode : -sys_data.s.values.mAh_AutoMode;
}

//	--- LOKALE FUNKTIONEN - bitte hier dokumentieren -----------------------------
int getSocAhRated(void)
{
    int64_t cellCapacitySeconds =  (int64_t)sys_data.s.parameter.cellCapacity * 60 * 60; // Umrechnung mAh zu mAs
	return (100000 * sys_data.s.values.mAsCounter)  / cellCapacitySeconds;
}


int getSocAhAuto(void)
{

	const int64_t _100mPercent = 100000LL;


	int64_t mAh_AutoMode = sys_data.s.values.mAh_AutoMode < 0 ? -sys_data.s.values.mAh_AutoMode : 0;
	int64_t tmp = 0LL;
	if (sys_data.s.values.detectedCapacity <= 0)
        {
          tmp = _100mPercent - (mAh_AutoMode * _100mPercent) / (int64_t)sys_data.s.parameter.cellCapacity;
        }
        else
        {
          tmp = _100mPercent - (mAh_AutoMode * _100mPercent) / (int64_t)sys_data.s.values.detectedCapacity;
	}

	if (tmp > _100mPercent) tmp = _100mPercent;
	else if (tmp <= 0) tmp = 0LL;
	return tmp;
 }


//	--- GLOBALE FUNKTIONEN - bitte in Header dokumentieren------------------------

void AH_COUNTER_Exec(void)
{
  double iBatDivIbatNenn = 0;
  double current = 0;
  double peukert = 0;
  double calcPow = 0;
  double cef = 0;
  double soc = 0;
  int64_t maxCurrentForBatteryFullDetection = 0;
  static int16_t batteryFullCounter = 0;
  static uint64_t totalDischarge = 0;
  static uint64_t totalCharge = 0;
  
  int64_t cellCapacitySeconds =  (int64_t)sys_data.s.parameter.cellCapacity * 60 * 60; // Umrechnung mAh zu mAs


  if (totalDischarge == 0) totalDischarge = sys_data.s.values.dischargeTotalAh * 3600000;
  if (totalCharge == 0) totalCharge = sys_data.s.values.chargeTotalAh * 3600000;


  int32_t realStrom = (int32_t)  sys_data.s.values.batteryCurrent - sys_data.s.parameter.extraDischargeStrom_mA;

  // bei Strom größer 0 -> Ladestrom CEF rechnen
  if (realStrom >= 0)
  {
	  //99 --> 99% --> 0.99
	  //if (sys_data.s.values.calculatedCEFAh <= 0)
	  //{
		cef =  sys_data.s.parameter.cef / 1000.0;
	  //}
	  //else
	  //{
	  //	cef =  sys_data.s.values.calculatedCEFAh / 1000.0;
	  //}
	  sys_data.s.values.batteryCurrentCorrected = realStrom * cef * (sys_data.s.values.peukertRemoveCorrectionFaktor/1000.0);
  }
  else // if (realStrom < 0)
  {	  // bei Strom kleiner 0 peukert rechnen
	  //int32_t ratedCurrent = sys_data.s.parameter.cellRatedCurrent * 1000;
	  int32_t ratedCurrent = sys_data.s.parameter.cellCapacity / sys_data.s.parameter.cellRatedDischargeTime;
	  

	  if (realStrom < -ratedCurrent) //ACHTUNG mit Minus das vorzeichen gedreht!
	  {
		  current = realStrom;
		  iBatDivIbatNenn = current / ratedCurrent;
		  iBatDivIbatNenn = -iBatDivIbatNenn;
		  peukert = (sys_data.s.parameter.peukert / 100.0);
		  calcPow = pow(iBatDivIbatNenn , peukert - 1.0);
		  sys_data.s.values.batteryCurrentCorrected = (current * calcPow);

	  }
	  else sys_data.s.values.batteryCurrentCorrected = realStrom;
  }

  
  // Counting negative current
  if (sys_data.s.values.batteryCurrent < 0)
  {
	  totalDischarge += -realStrom;
	  sys_data.s.values.dischargeTotalAh = totalDischarge / 3600000; //Umrechnung von mAs auf Ah

	  sys_data.s.values.fullCyclesCnt = (uint16_t) ((sys_data.s.values.dischargeTotalAh * 1000) / sys_data.s.parameter.cellCapacity);
  }
  else
  {
	  totalCharge += realStrom;
	  sys_data.s.values.chargeTotalAh = totalCharge / 3600000; //Umrechnung von mAs auf Ah
  }


  // Aufsummieren
  sys_data.s.values.mAsCounter += sys_data.s.values.batteryCurrentCorrected;
  sys_data.s.values.mAs_AutoMode += sys_data.s.values.batteryCurrentCorrected;
  
  sys_data.s.values.mAh_AutoMode = sys_data.s.values.mAs_AutoMode / 3600LL;
  


  if ((sys_data.s.values.soc > 0) || startMeasurementDischarge)
  {
	sys_data.s.values.mAsCounterUncorrected +=  realStrom;
	sys_data.s.values.mAhCounterUncorrected = sys_data.s.values.mAsCounterUncorrected / 3600;
  }

  
  sys_data.s.values.mAs_AutoModeU += sys_data.s.values.batteryCurrent;
  sys_data.s.values.mAh_AutoModeU = sys_data.s.values.mAs_AutoModeU / 3600LL;
  if (sys_data.s.values.mAh_AutoModeU != 0)
  {
	sys_data.s.values.peukertRemoveCorrectionFaktor = 1000 * sys_data.s.values.mAh_AutoMode /  sys_data.s.values.mAh_AutoModeU;
  }
  else
  {
	sys_data.s.values.peukertRemoveCorrectionFaktor=1000;
  }
  
  // Begrenzen, Batterie darf nicht über 100% gehen
  if (sys_data.s.values.mAsCounter > cellCapacitySeconds) sys_data.s.values.mAsCounter = cellCapacitySeconds;

  if (sys_data.s.values.mAs_AutoMode > 0)
  {
	sys_data.s.values.mAs_AutoMode = 0;
	sys_data.s.values.mAh_AutoMode = 0;
	sys_data.s.values.mAs_AutoModeU = 0;
	sys_data.s.values.mAh_AutoModeU = 0;


  }

  //Prüfe Battery Voll Bedinungen
  maxCurrentForBatteryFullDetection = sys_data.s.parameter.cellCapacity * sys_data.s.parameter.iBatFull / 100.0;

  if (sys_data.s.values.batteryVoltage > sys_data.s.parameter.uBatFull && sys_data.s.values.batteryCurrent <  maxCurrentForBatteryFullDetection)
  {
    batteryFullCounter++;
	if (batteryFullCounter > sys_data.s.parameter.tBatFull) batteryFullCounter = sys_data.s.parameter.tBatFull;
  }
  else
  {
    batteryFullCounter = 0;
  }

  if (batteryFullCounter >= sys_data.s.parameter.tBatFull)
  {
    sys_data.s.values.mAsCounter = cellCapacitySeconds;
	sys_data.s.values.mAs_AutoMode = 0;
	sys_data.s.values.mAh_AutoMode = 0;
	// Here we can set Wh to max
	WH_COUNTER_SetToMax();

	//und wir starten eine neue Battery Kapazitäts und Energiemessung
	if (startMeasurementDischarge == 0)
	{
	   
	  startMeasurementDischarge = 1;
	}

	if (startMeasurementCEF == 1)
	{
	  startMeasurementCEF = 0;
	  sys_data.s.values.calculatedCEFAh = (1000LL * sys_data.s.values.detectedCapacityAtActualCRate * 3600LL) / sys_data.s.values.mAsCounterUncorrected ;
	  sys_data.s.values.calculatedCEFWh = (1000LL * sys_data.s.values.detectedEnergyAtActualCRate * 3600LL) / sys_data.s.values.mWsCounterUncorrected;
	  printf("Time %d: Batterie Full event mAhCarged=%d, tCharge=%d, cefAh=%d, cefWh=%d, u=%d, i=%d\r\n",sys_data.s.values.onTime, sys_data.s.values.mAhCounterUncorrected, sys_data.s.values.lastTimeVbatEmpty, sys_data.s.values.calculatedCEFAh, sys_data.s.values.calculatedCEFWh, sys_data.s.values.batteryVoltage, sys_data.s.values.batteryCurrent);
	}
	else {
	  
	  //Messung CEF ferig, halter Zähler auf 0 solange Batterie voll, damit die Messung der Kapazität/Energy bei aktuellen Entladestrom korrekt startet
	  sys_data.s.values.mAsCounterUncorrected = 0;
	  sys_data.s.values.mAhCounterUncorrected = 0;
	  sys_data.s.values.mWsCounterUncorrected = 0;
	}
	
  }

  sys_data.s.values.mAhCounter = sys_data.s.values.mAsCounter / 3600LL;

  // --- BATTERY LEER ERKENNUNG
  static uint16_t cnt;
  if (sys_data.s.parameter.batteryEmptyDetectionMode == 0)
  {
    if (sys_data.s.values.batteryVoltage < sys_data.s.values.uBatEmptyTempComp && sys_data.s.values.batteryVoltage > 1000) // Verhindert das beim abziehen der Sense ein Batt Empty erkannt wird
    {
  	  cnt++;
  	  if (cnt >= 10)  
  	  {
  		  cnt = 10; //sys_data.s.parameter.tBatFull;

  		  if ( (startMeasurementDischarge == 1) && (sys_data.s.values.lastTimeVbatFull >= 1200U) && (sys_data.s.values.lastTimeVbatFull <= 200U * 3600U)) 	// This line prevents from very high discharge-currents to be used to estimate battery capacity
  		  {
  			  
  			  AH_COUNTER_SetDetectedAh();
  			  WH_COUNTER_SetDetectedEnergy();
			  sys_data.s.values.detectedCapacityAtActualCRate = -sys_data.s.values.mAsCounterUncorrected / 3600;
			  sys_data.s.values.detectedEnergyAtActualCRate = -sys_data.s.values.mWsCounterUncorrected /3600;	  	  
			  printf("Time %d: Empty event(1), cn=%d, ca=%d, tDischarge=%d, u=%d, i=%d\r\n",sys_data.s.values.onTime, sys_data.s.values.detectedCapacity, sys_data.s.values.detectedCapacityAtActualCRate,  sys_data.s.values.lastTimeVbatFull, sys_data.s.values.batteryVoltage, sys_data.s.values.batteryCurrent);
		  }
  		  sys_data.s.values.lastTimeVbatEmpty = 0U;

		  
		  //Messung wurde gespeichert (bzw. verworfen). Nächste Messung nach Aufladung
		  startMeasurementDischarge = 0;
		  			

		  //Batterie ist Leer, wir können die Messung der Ladung beginnen
		  startMeasurementCEF = 1;

		  sys_data.s.values.mAsCounterUncorrected = 0;
		  sys_data.s.values.mAhCounterUncorrected = 0;
		  sys_data.s.values.mWsCounterUncorrected = 0;

  	  }
    }
    else 
    {
      cnt = 0;
    }
  }
  else
  {
	// Neuer Modus. Spannungsmessung wird ignoriert. Erkannt wird Batt Leer mit LVP Signal von LiPro
	// OVP darf nicht ausgehen, sonst handelt es sich um ein Temperaturabschaltung oder ein andere Fehler
	// 1000mV als Schwelle um sicher vor rauschen um den Nullpunkt zu seinzu sein
    if ((sys_data.s.values.ovp_sense > 1000) && (sys_data.s.values.lvp_sense < 1000)) 
    {
  	  cnt++;
  	  if (cnt >= 10) 
  	  {
  		  cnt = 10; //sys_data.s.parameter.tBatFull;

  		  if ( (startMeasurementDischarge == 1) && (sys_data.s.values.lastTimeVbatFull >= 3600U) && (sys_data.s.values.lastTimeVbatFull <= 240U * 3600U)) 	// This line prevents from very high discharge-currents to be used to estimate battery capacity
  		  {
  			  
			  AH_COUNTER_SetDetectedAh();
  			  WH_COUNTER_SetDetectedEnergy();
			  sys_data.s.values.detectedCapacityAtActualCRate = -sys_data.s.values.mAsCounterUncorrected/ 3600;
			  sys_data.s.values.detectedEnergyAtActualCRate = -sys_data.s.values.mWsCounterUncorrected / 3600;	
			  printf("Time %d: Empty event(2), cn=%d, ca=%d, tDischarge=%d, u=%d, i=%d\r\n",sys_data.s.values.onTime, sys_data.s.values.detectedCapacity, sys_data.s.values.detectedCapacityAtActualCRate,  sys_data.s.values.lastTimeVbatFull, sys_data.s.values.batteryVoltage, sys_data.s.values.batteryCurrent);
		  }

		  
  		  
		  sys_data.s.values.lastTimeVbatEmpty = 0U;
		  
		  //Messung wurde gespeichert (bzw. verworfen). Nächste Messung nach Aufladung
		  startMeasurementDischarge = 0;		

		  //Batterie leer wir können mit der Messung der Ladung beginnen
		  startMeasurementCEF = 1;

		  
		  sys_data.s.values.mAsCounterUncorrected = 0;
		  sys_data.s.values.mAhCounterUncorrected = 0;
		  sys_data.s.values.mWsCounterUncorrected = 0;
  	  }
    }
    else 
    {
      cnt = 0;
    }
  }


  switch (sys_data.s.parameter.socCalcMode)
  {
	  case SOC_CALC_MODE_AH_RATED:		sys_data.s.values.soc = getSocAhRated();			  break;
	  case SOC_CALC_MODE_AH_AUTO:		sys_data.s.values.soc = getSocAhAuto();				  break;
	  case SOC_CALC_MODE_WH_RATED:		sys_data.s.values.soc = WH_COUNTER_GetSoCManual();	  break;
	  case SOC_CALC_MODE_WH_AUTO:		sys_data.s.values.soc = WH_COUNTER_GetSoCAuto();	  break;
	  case SOC_CALC_MODE_WH_AUTO_TEMP:	sys_data.s.values.soc = WH_COUNTER_GetSoCAutoTemp();  break;
	  default: sys_data.s.values.soc = 0;
  }

	sys_data.s.values.soc0 = getSocAhRated()/100;			  
	sys_data.s.values.soc1 = getSocAhAuto()/100;				  
	sys_data.s.values.soc2 = WH_COUNTER_GetSoCManual()/100;	  
	sys_data.s.values.soc3 = WH_COUNTER_GetSoCAuto()/100;	  
	sys_data.s.values.soc4 = WH_COUNTER_GetSoCAutoTemp()/100;  
  


}