#include "soc_estimation.h"

#include <stdint.h>
#include "slave_monitoring.h"
#include "stm32f3xx_hal.h"


//------------------------------------Battery RC and OCV-SoC Parameters-----------------------------------------
//@Note Parameters were obtained by EIS Measurements at the start of the season
//If the errror with this values is to large, consider retesting some cells
const float SOC[N_MODELPARAMETERS]={0,0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1};
const float R0[N_MODELPARAMETERS]={0.0089,0.0087,0.0090,0.0087,0.0087,0.0087,0.0088,0.0088,0.0087,0.0088,0.0089};
const float R1[N_MODELPARAMETERS]={0.0164,0.0063,0.0050,0.0055,0.0051,0.0052,0.0057,0.0048,0.0059,0.0055,0.0061};
const float C1[N_MODELPARAMETERS]={2.5694,0.2649,0.2876,0.2594,0.2415,0.2360,0.2946,0.2558,0.2818,0.2605,0.2763};
const float OCV_Data[N_MODELPARAMETERS]={2.762504,3.326231,3.460875,3.57681,3.655326,3.738444,3.835977,3.925841,4.032575,4.078275,4.191449};
const float SOE_Data[N_MODELPARAMETERS]={0.0,0.079358,0.165140,0.256008,0.348836,0.445961,0.549115,0.655642,0.769677,0.875699,1.0};
//---------------------------------------------------------------------------------------------------------------



float soc_approxparameterbysoc(float,float*, uint8_t);
float soc_approxsocbyocv(float);
uint8_t current_soc;
float current_floatsoc;

float batterycapacity;


/**
 * @brief This Function initializes the SoC Prediction
 * @note Because SoC is initalized using the OCV-Curve of the Cell, it is necessary to obtain a valid value
 * for the lowest cell voltage before calling this function
 */
void soc_init() {
  float minvoltage = ((float)slaves_get_minimum_voltage())/1000;
  current_floatsoc = soc_approxsocbyocv(minvoltage);
  batterycapacity = BATTERYCAPACITYAs*current_floatsoc;
  current_soc = (uint8_t)(current_floatsoc*100);
}

/**
 * @brief Update Function for the State of Charge. Call this Function every time the shunt sends a new current
 * @note The SoC Prediction works using a Coulomb Counter to track the SoC. Alternativly and maybe more elegant
 * would be to track the SoC using the integrated current counter of the shunt.
 * @param shunt_current 
 */
void soc_update(int32_t shunt_current) {
  // TODO
  static uint32_t lasttick = 0;

  if(lasttick != 0)
  {
    uint32_t dt = HAL_GetTick() - lasttick;
    batterycapacity += batterycapacity + ((float) dt*shunt_current)/1000;
    current_floatsoc = batterycapacity/BATTERYCAPACITYAs;
    current_soc = (uint8_t) (current_floatsoc*100);
  }
  
  lasttick=HAL_GetTick();

}

float soe_update()
{
  return soc_approxparameterbysoc(current_floatsoc, SOE_Data, N_MODELPARAMETERS);
}
void soap_update()
{
  //
}

float soc_approxparameterbysoc(float soc,float* lut, uint8_t lutlen)
{
  
  uint8_t idx = (uint8_t) (soc*(lutlen-1));

  if(idx == (lutlen-1))
    return lut[lutlen-1];

  float linapprox = 10*(soc-(((float)idx)/((float)(lutlen-1))))*(lut[idx+1]-lut[idx]);
  linapprox += lut[idx];
  return linapprox;
}

float soc_approxsocbyocv(float ocv)
{

  if(ocv < OCV_Data[0])
    return 0;

  if(ocv > OCV_Data[N_MODELPARAMETERS])
    return 1;
  //Iterate through OCV Lookup
  uint8_t ocvindex = 0;
  for(uint8_t i = 0; i < (N_MODELPARAMETERS-1);i++)
  {
    if((OCV_Data[i] <= ocv) && (OCV_Data[i+1] > ocv))
    {
      ocvindex = i;
    }
  }
  
  float m = (ocv-OCV_Data[ocvindex])/(OCV_Data[ocvindex+1]-OCV_Data[ocvindex]);
  
  float soc = (SOC[ocvindex+1] - SOC[ocvindex])*m + SOC[ocvindex];

  return soc;
  

}