#include "SoC_Estimation.h"

#include "CAN_Communication.h"
#include "SPI_Communication.h"
#include "util.h"

#include <stdint.h>

uint8_t current_soc = 0;

static const float internal_resistance_curve_x[] = {2.0, 4.12};
static const float internal_resistance_curve_y[] = {0.0528, 0.0294};
#define INTERNAL_RESISTANCE_CURVE_PTS                                          \
  (sizeof(internal_resistance_curve_x) / sizeof(*internal_resistance_curve_x))
static const float soc_ocv_x[] = {2.1, 2.9,  3.2, 3.3,  3.4,
                                  3.5, 3.68, 4.0, 4.15, 4.2};
static const float soc_ocv_y[] = {0,     0.023, 0.06,  0.08,  0.119,
                                  0.227, 0.541, 0.856, 0.985, 1.0};
#define SOC_OCV_PTS (sizeof(soc_ocv_x) / sizeof(*soc_ocv_x))

static int start_soc_known = 0;
static float start_soc;
static float start_as;
static float min_cell_voltage;

static void estimate_start_soc_off();
static void estimate_start_soc_on();

void estimate_soc() {
  uint16_t min_v = 0xFFFF;
  for (int slave = 0; slave < NUMBEROFSLAVES; slave++) {
    for (int cell = 0; cell < N_CELLS_SERIES; cell++) {
      uint16_t v = slaves[slave].cellVoltages[cell];
      if (v < min_v) {
        min_v = v;
      }
    }
  }
  min_cell_voltage = min_v * CELL_VOLTAGE_CONVERSION_FACTOR;

  if (shuntcurrent < SOCE_SHUNT_CURRENT_OFF_THRESH) {
    estimate_start_soc_off();
  } else if (!start_soc_known) {
    estimate_start_soc_on();
  }

  float soc = start_soc - (shuntampereseconds - start_as) / BATTERY_CAPACITY;
  current_soc = soc * 255;
}

static void estimate_start_soc_on() {
  float r_i = interp(INTERNAL_RESISTANCE_CURVE_PTS, internal_resistance_curve_x,
                     internal_resistance_curve_y, min_cell_voltage);
  float i = shuntcurrent / N_CELLS_PARALLEL;
  float ocv = min_cell_voltage - i * r_i;
  start_soc = calculate_soc_for_ocv(ocv);
  start_as = shuntampereseconds;
  start_soc_known = 1;
}

static void estimate_start_soc_off() {
  start_soc = calculate_soc_for_ocv(min_cell_voltage);
  start_as = shuntampereseconds;
  start_soc_known = 1;
}

float calculate_soc_for_ocv(float ocv) {
  return interp(SOC_OCV_PTS, soc_ocv_x, soc_ocv_y, ocv);
}