#include <WiFi.h>
#include <PubSubClient.h>

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqttServer = "thingsboard.cloud";
const int mqttPort = 1883;
const char* accessToken = "lftfmnn2d8jh0dbhx0ad";

const int LM35Pin = 34;  
const int numReadings = 10;  
int readings[numReadings];   
int readingIndex = 0;  
int total = 0;  
float averageTemperature = 0.0;  
float KalmanFilterData;
float Xt, Xt_update, Xt_prev;
float Pt, Pt_update, Pt_prev;
float Kt, R, Q;

WiFiClient espClient;
PubSubClient client(espClient);

void setup_wifi() {
  Serial.begin(9600);
  pinMode(LM35Pin, INPUT);
  analogReadResolution(12);
  
  WiFi.begin(ssid, password); 
  Serial.println("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected to WiFi");
}

void callback(char* topic, byte* payload, unsigned int length) {
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("Connecting to MQTT...");
    if (client.connect("ESP32Client", accessToken, "")) {
      Serial.println("Connected to MQTT");
    } else {
      Serial.print("Failed, rc=");
      Serial.print(client.state());
      Serial.println(" Trying again in 5 seconds");
      delay(5000);
    }
  }
}

void setup() {
  setup_wifi();
  client.setServer(mqttServer, mqttPort);
  client.setCallback(callback);
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  int sensorValue = analogRead(LM35Pin);
  float temperatureC = (sensorValue / 4095.0) * 330.0;

  Serial.print("Temperature: ");
  Serial.print(temperatureC);   // print the temperature in Celsius
  Serial.print("°C");
  Serial.print(" ~ ");

  Xt_update = Xt_prev;
  Pt_update = Pt_prev + Q;
  Kt = Pt_update / (Pt_update + R);
  Xt = Xt_update + (Kt * (temperatureC - Xt_update));
  Pt = (1 - Kt) * Pt_update;
 
  Xt_prev = Xt;
  Pt_prev = Pt;
  KalmanFilterData = Xt;

  total = total - readings[readingIndex] + sensorValue;
  readings[readingIndex] = sensorValue;
  averageTemperature = (total / numReadings) / 4095.0 * 330.0;
  Serial.print("Average Temperature: ");
  Serial.println(averageTemperature);
  readingIndex = (readingIndex + 1) % numReadings;

  char payload[100];
  snprintf(payload, sizeof(payload), "{\"temperature\":%.2f,\"kalman\":%.2f,\"average\":%.2f}", temperatureC, KalmanFilterData, averageTemperature);

  if (client.publish("v1/devices/me/telemetry", payload)) {
    Serial.println("Data published to ThingsBoard");
  } else {
    Serial.println("Failed to publish data");
  }

  delay(1000);
}