#include <ArduinoJson.h>
#include <ArduinoJson.hpp>
#include <WiFi.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <SensirionI2CScd4x.h>  // Include the SCD41 library

SensirionI2CScd4x scd4x;  // Create an instance of the SCD41 sensor

const char* ssid = "xxxx";
const char* password = " ";
const char* mqtt_server = "xxxx";

WiFiClient espClient;
PubSubClient client(espClient);
unsigned long lastMsg = 0;
#define MSG_BUFFER_SIZE (50)
char msg[MSG_BUFFER_SIZE];
int value = 0;

void setup_wifi() {
  delay(100);
  // We start by connecting to a WiFi network
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.init(AP_STA_MODE);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  randomSeed(micros());

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void callback(char* topic, byte* payload, unsigned int length) {
  // Empty function, you can add your logic here
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Create a random client ID
    String clientId = "ESP8266Client-";
    clientId += String(random(0xffff), HEX);
    // Attempt to connect
    if (client.connect(clientId.c_str())) {
      Serial.println("connected");
      // Once connected, publish an announcement...
      client.publish("outTopic", "hello world");
      // ... and resubscribe
      client.subscribe("inTopic");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  while (!Serial);
  Serial.println(F("SCD41 test"));

  Wire.begin();

  uint16_t error;
  char errorMessage[256];

  scd4x.begin(Wire);

  // Stop any previous measurement if it exists
  error = scd4x.stopPeriodicMeasurement();
  if (error) {
    Serial.print("Error trying to stop periodic measurement: ");
    Serial.println(error);
  }

  // Start the periodic measurement
  error = scd4x.startPeriodicMeasurement();
  if (error) {
    Serial.print("Error starting periodic measurement: ");
    Serial.println(error);
  } else {
    Serial.println("Measurement started, waiting for data...");
  }

  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();
  
  StaticJsonDocument<64> doc;
  char output[100];

  long now = millis();
  if (now - lastMsg > 5000) {
    lastMsg = now;

    uint16_t co2;
    float temperature;
    float humidity;

    uint16_t error = scd4x.readMeasurement(co2, temperature, humidity);
    if (error) {
      Serial.print("Error reading measurement: ");
      Serial.println(error);
    } else {
      // Only proceed if a valid measurement is obtained
      if (co2 > 0) {
        doc["c"] = co2;
        doc["t"] = temperature;
        doc["h"] = humidity;

        // Prepare and publish the JSON data
        serializeJson(doc, output);
        Serial.println(output);
        client.publish("/home/sensors", output);
        Serial.println("Data sent");
      } else {
        Serial.println("Invalid sample detected, skipping...");
      }
    }
  }
}