//Created by Barbu Vulc.
/* 
 * I wanted to apply the Kalman Filter algorithm for a specific case:
 * the State-of-charge estiomation of a sand battery.
 * Sand battery, as the name suggests, is an energy storage environment made of sand.
 * Sand batteries are an alternative to Li-Ion or other chemical energy storage
 * solutions, which could be used either in home energy storage systems or industry.
 */
//Libraries:
#include <DHT.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);

#define DHTPIN 2        // DHT22 connected to digital pin D2!
#define DHTTYPE DHT22   // Define the sensor
DHT dht(DHTPIN, DHTTYPE);

//Kalman Filter variables
float SoC = 0.5;  // State-of-charge, 50% initially!
float P = 1;      
float Q = 0.01;   // Noise process
float R = 0.1;    // Noise measure

//The Kalman Filter function:
float kalmanFilter(float z, float x, float P) {
  float x_pred = x;
  float P_pred = P + Q;
  float K = P_pred / (P_pred + R);
  float x_new = x_pred + K * (z - x_pred);
  float P_new = (1 - K) * P_pred;
  P = P_new;
  return x_new;
}

void setup() {
  Serial.begin(9600);
  dht.begin();
  //LCD initialization!
  lcd.init(); lcd.backlight();
}

void loop() { 
    float temperature = dht.readTemperature();  // Read temperature
    if (isnan(temperature)) {
        Serial.println("Error reading DHT22!");
        return;
    }

    float measured_SoC = (temperature / 50.0);
    SoC = kalmanFilter(measured_SoC, SoC, P);  //Apply Kalman Filter...

    if(SoC * 100 > 100.00) {
      lcd.setCursor(0, 0);
      lcd.print("State-of-charge:");
      lcd.setCursor(0, 1);
      lcd.print("Fully charged!");
    } else if(SoC * 100 <= 0.00) {
      lcd.setCursor(0, 0);
      lcd.print("State-of-charge:");
      lcd.setCursor(0, 1);
      lcd.print("Depleted!     ");
    } else {
      Serial.print("Temp: ");
      Serial.print(temperature);
      Serial.print("°C; ");
      Serial.print("SoC: ");
      Serial.print(SoC * 100);
      Serial.println("%");
      lcd.setCursor(0, 0);
      lcd.print("State-of-charge:");
      lcd.setCursor(0, 1);
      lcd.print(SoC * 100);
      lcd.setCursor(6, 1);
      lcd.print("        ");
    }
}