#include <Arduino.h>

// Parameters for battery model and EKF
#define BATTERY_CAPACITY_AH 5.2 // Battery capacity in Amp-hours
#define SAMPLING_INTERVAL 1000 // Sampling interval in milliseconds

const int voltagePin = A0; // Analog pin for battery voltage
const int currentPin = A1; // Analog pin for current sensor

// OCV-SoC Curve 
float ocv_soc_curve[11][2] = {
    {16.76, 100}, // OCV at full charge
    {15.76, 70},
    {16.38, 90},
    {15.97, 80},  
    {15.49, 60},
    {15.3, 50},
    {15.2, 40},
    {15.14, 30},
    {15.02, 20},
    {14.81, 10},
    {13.48, 0}   // OCV at low charge
};

// State variables
float soc = 100; // Initial guess for State of Charge (percentage)
float batteryCapacityAh = BATTERY_CAPACITY_AH;
float totalChargeInCoulombs = batteryCapacityAh * 3600; // Total charge in coulombs
float chargeUsedCoulombs = 0;
float previousCurrent = 0;

// EKF parameters
float stateEstimate = soc; // Initial state estimate (SoC)
float errorCovariance = 1; // Initial error covariance
float processNoise = 0.007; // Process noise covariance (adjust based on system behavior)
float measurementNoise = 0.001; // Measurement noise covariance (adjust based on sensor noise)

// Functions for reading sensor data
float readBatteryVoltage() {
  int sensorValue = analogRead(voltagePin);
  // Convert to voltage (based on your voltage divider configuration)
  float batteryVoltage = sensorValue * (5.0 / 1023.0) * 10;
  return batteryVoltage;
}

float readBatteryCurrent() {
  int sensorValue = analogRead(currentPin);
  float current = (sensorValue - 512) * (5.0 / 1023.0) * 30;
  return current;
}

// Linear interpolation for the OCV-SoC curve
float interpolate(float x, float x1, float y1, float x2, float y2) {
  return y1 + (x - x1) * (y2 - y1) / (x2 - x1);
}

float estimateSoCfromOCV(float ocv) {
  for (int i = 0; i < 10; i++) { // Loop through the curve
    if (ocv >= ocv_soc_curve[i + 1][0]) {
      return interpolate(ocv, ocv_soc_curve[i][0], ocv_soc_curve[i][1], ocv_soc_curve[i + 1][0], ocv_soc_curve[i + 1][1]);
    }
  }
  return 0; // Return 0% if OCV is below the lowest value
}

// EKF update step with OCV-SoC curve
void updateEKF(float measurement, float voltage) {
  // Predict
  float statePrediction = stateEstimate - previousCurrent * (SAMPLING_INTERVAL / 1000.0); // Coulomb Counting
  float errorPrediction = errorCovariance + processNoise;

  // Estimate SoC from OCV
  float ocvBasedSoC = estimateSoCfromOCV(voltage);

  // Kalman Gain
  float kalmanGain = errorPrediction / (errorPrediction + measurementNoise);

  // Measurement Update
  stateEstimate = statePrediction + kalmanGain * (ocvBasedSoC - statePrediction); // Update with OCV-SoC
  errorCovariance = (1 - kalmanGain) * errorPrediction;

  previousCurrent = measurement; // Update for next step
}

void setup() {
  Serial.begin(9600);
}

void loop() {
  static unsigned long previousMillis = 0;
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= SAMPLING_INTERVAL) {
    previousMillis = currentMillis;

    // Read battery current and voltage
    float batteryCurrent = readBatteryCurrent();
    float batteryVoltage = readBatteryVoltage();

    // Update EKF with current and voltage measurement
    updateEKF(batteryCurrent, batteryVoltage);

    Serial.print("Battery Voltage: ");
    Serial.print(batteryVoltage);
    Serial.print(" V, Battery Current: ");
    Serial.print(batteryCurrent);
    Serial.print(" A, Estimated SoC: ");
    Serial.print(stateEstimate);
    Serial.println(" %");
  }
}