#include "sysdata.h"
#include "esr.h"
#include <stdlib.h>
#include "main.h"
#include "battery_voltage.h"
#include "fast_current.h"



int32_t current_buffer[SAMPLE_ARRAY_SIZE];  
int32_t voltage_buffer[SAMPLE_ARRAY_SIZE];


extern uint16_t adc1Data[SAMPLE_ARRAY_SIZE];
extern uint16_t adc2Data[SAMPLE_ARRAY_SIZE];
extern uint16_t adc4Data[SAMPLE_ARRAY_SIZE];


int16_t ESR_Exec(void)
{
 

  static int32_t last_refresh;
  int x;

  //Anzeige vor wieviel Sekunden zuletzt aktualisiert wurd.
  sys_data.s.values.esrCalcTime = sys_data.s.values.onTime - last_refresh;

  for (x=SAMPLE_ARRAY_SIZE-1; x>0; x--)
  {
    current_buffer[x] = current_buffer[x-1];
    voltage_buffer[x] = voltage_buffer[x-1];
  }

  // Neue Werte ins array aufnehmen
  current_buffer[0] = sys_data.s.values.batteryCurrent;
  voltage_buffer[0] = sys_data.s.values.batteryVoltage;


  //Suche Min und max werte im Array
  int32_t minU=INT32_MAX;
  int32_t maxU=0;
  int32_t minI=INT32_MAX;
  int32_t maxI=0;
  int32_t minIPos = -1;
  int32_t maxdIPos = -1;
  int32_t minUPos = -1;
  int32_t maxUPos = -1;

  //Suche min und max werte
  for (x=0; x < SAMPLE_ARRAY_SIZE; x++)
  {
     if (abs(current_buffer[x]) < minI)  { minI = abs(current_buffer[x]); minIPos  = x; }
     if (abs(current_buffer[x]) >= maxI) { maxI = abs(current_buffer[x]); maxdIPos = x; } 
     if (abs(voltage_buffer[x]) < minU)  { minU = abs(voltage_buffer[x]); minUPos = x; }
     if (abs(voltage_buffer[x]) > maxU)  { maxU = abs(voltage_buffer[x]); maxUPos = x; }
  }

  
  //Suche Zeitpunkt der größten Änderung in I

  //Delta berechnen
  int32_t dI = abs (maxI - minI);
  int32_t dU = abs (maxU - minU);

  //Minimale Belastung Prüfen ob es genügent Änderungen gab
  // 1/20 des Nennstroms
  // Bei 100Ah Batterie mit 0,5 Std discharge --> 50A --> /20 =2,5 A
  int32_t min_dI;
  min_dI = sys_data.s.parameter.cellCapacity /  sys_data.s.parameter.cellRatedDischargeTime; //Nennlaststrom  in mA
  min_dI = min_dI / 20 ;

  int32_t min_dU = 25;
  
  if( dI < min_dI)
  {
  
    return -1;
  }

  //printf("dI change!\r\n");

  if (dU < min_dU) {
    return -2;
  }

  //printf("dU change!\r\n");

 
  int32_t dIMax=-1;
  int32_t dIx=-1;;
  int32_t dIMaxPos=-1;
 
  for (x=0; x < (SAMPLE_ARRAY_SIZE-1); x++)
  {
    dIx = abs(current_buffer[x+1] - current_buffer[x]); 
    if (dIx > dIMax) { dIMax = dIx; dIMaxPos = x; } 
  }



  if (dIMaxPos == SAMPLE_ARRAY_SIZE / 2)
  {
    //ESR berechnen!
    sys_data.s.values.esr = ( (double)dU / (double) dI) * 10000;
    last_refresh = sys_data.s.values.onTime;


    for (x=0; x < SAMPLE_ARRAY_SIZE; x++)
    {
      sys_data.s.values.current_buffer[(SAMPLE_ARRAY_SIZE-1)-x] = adc1Data[x];
      sys_data.s.values.voltage_buffer[(SAMPLE_ARRAY_SIZE-1)-x] = adc2Data[x];
    }


    
  }
  return 0;
}


int16_t ESR_FAST_Exec(void)
{
 
    
  static int32_t last_refresh;
  int x;

  //Anzeige vor wieviel Sekunden zuletzt aktualisiert wurd.
  //Aktuell erfolgt nur die Anze der low speed Methode
  //sys_data.s.values.esrCalcTime = sys_data.s.values.onTime - last_refresh;



  //Suche Min und max werte im Array
  int32_t minUBatt=INT32_MAX;
  int32_t maxUBatt=0;
  int32_t minUOut=INT32_MAX;
  int32_t maxUOut=0;
  int32_t minI=INT32_MAX;
  int32_t maxI=0;
  int32_t minIPos = -1;
  int32_t maxdIPos = -1;
  int32_t minUBattPos = -1;
  int32_t maxUBattPos = -1;
  int32_t minUOutPos = -1;
  int32_t maxUOutPos = -1;

  //Suche min und max werte
  for (x=0; x < SAMPLE_ARRAY_SIZE; x++)
  {
     if (adc1Data[x] < minI)  { minI = adc1Data[x]; minIPos  = x; }
     if (adc1Data[x] >= maxI) { maxI = adc2Data[x]; maxdIPos = x; }  
     if (adc2Data[x] < minUBatt)  { minUBatt = adc2Data[x]; minUBattPos = x; }
     if (adc2Data[x] > maxUBatt)  { maxUBatt = adc2Data[x]; maxUBattPos = x; }
	 if (adc4Data[x] < minUOut)  { minUOut = adc4Data[x]; minUOutPos = x; }
     if (adc4Data[x] > maxUOut)  { maxUOut = adc4Data[x]; maxUOutPos = x; }
  }

  


  //Delta berechnen
  int32_t dI = maxI - minI;
  
  //Nehme nicht mehr die gesamte maximale Differenz der Spannungen, sondern nehme das delt U wo auch das Delta I gemessen wurde
  //Funktioniert nur bei Synchroner Messug von Strom und Spannung
  //int32_t dU = maxU - minU;
  int32_t dUBatt = adc2Data[maxdIPos]  - adc2Data[minIPos];
  int32_t dUOut =  adc4Data[maxdIPos]  - adc4Data[minIPos];

  //Umrechnung in mV / mA
  dI = dI * ((double) VREF / FAST_CURRENT_SHUNT_RESISTOR /  FAST_CURRENT_I_SENSE_GAIN /  FAST_CURRENT_ADC_RESOLUTION);
  dI = dI * (sys_data.s.parameter.batteryCurrentGainCorrectionFaktor / 1000000.0);

  dUBatt = dUBatt  * (double )VREF * BATTERY_VOLTAGE_VOLTAGE_DIVIDER / BATTERY_VOLTAGE_ADC_RESOLUTION ;
  dUOut  = dUOut  * (double )VREF * BATTERY_VOLTAGE_VOLTAGE_DIVIDER / BATTERY_VOLTAGE_ADC_RESOLUTION ;

  //Minimale Belastung Prüfen ob es genügent Änderungen gab
  // 1/20 des Nennstroms
  // Bei 100Ah Batterie mit 0,5 Std discharge --> 50A --> /20 =2,5 A
  int32_t min_dI;
  min_dI = sys_data.s.parameter.cellCapacity /  sys_data.s.parameter.cellRatedDischargeTime; //Nennlaststrom  in mA
  min_dI = min_dI / 10 ;
 

  int32_t min_dU = 10;
  
  if( abs(dI) < min_dI)
  {
  
    return -1;
  }

  //printf("dI change!\r\n");

  //if (abs(dU) < min_dU) {
  //  return -2;
  //}

  //printf("dU change!\r\n");

 
  int32_t dIMax=-1;
  int32_t dIx=-1;;
  int32_t dIMaxPos=-1;
 


  //Finde Position der flanke
  for (x=0; x < (SAMPLE_ARRAY_SIZE-1); x++)
  {
    dIx = adc1Data[x+1] - adc1Data[x]; 
    if (dIx > dIMax) { dIMax = dIx; dIMaxPos = x; } 
  }

  


  //ESR berechnen!
  sys_data.s.values.esr_fast = ( (double)dUBatt / (double) dI) * 10000;
  sys_data.s.values.esr =	   ( (double)dUOut  / (double) dI) * 10000;
  last_refresh = sys_data.s.values.onTime;
	

  for (x=0; x < SAMPLE_ARRAY_SIZE; x++)
  {
	sys_data.s.values.current_buffer_fast[x] = (int32_t) adc1Data[x] - FAST_CURRENT_ADC_OFFSET  ;
	sys_data.s.values.voltage_buffer_fast[x] = (int32_t) adc2Data[x] - BATTERY_VOLTAGE_ADC_OFFSET ;
  }




  
  return 0;    
}