diff --git a/platformio.ini b/platformio.ini
index 4a6cf5fd163c43526d5b7e736a97a839cc442072..a33fdefaaff5de292b661677a76f1e9491b6b65e 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -26,6 +26,7 @@ lib_deps =
     avaldebe/PMSerial @ ^1.1.1
     boschsensortec/BSEC Software Library @ ^1.6.1480
     knolleary/PubSubClient @ ^2.8
+    spacehuhn/SimpleMap @ ^1.0.0
 
 [env:d1_mini_ota]
 extends = env:d1_mini
diff --git a/src/main.cpp b/src/main.cpp
index 71889a8e4d3ddcee6424ada7aaf7b974e2d1594e..c499047da0bfb1815af71155e52fdfec87ca5e00 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -11,6 +11,7 @@
 #include <PMserial.h>
 #include <bsec.h>
 #include <PubSubClient.h>
+#include <SimpleMap.h>
 
 #define PMS_TX D3
 #define PMS_RX D4
@@ -24,8 +25,7 @@ Bsec iaqSensor;
 PubSubClient mqttClient(wifiClient);
 IPAddress mqttServer;
 
-long checkMillis = 0;
-long checkInterval = 5000; // 5s
+SimpleMap<String, double> *sensorData;
 
 void logLine(String line, bool newline = true)
 {
@@ -67,6 +67,13 @@ void setupWifi()
   }
 }
 
+void setupMDNS() {
+  if (!MDNS.begin(HOSTNAME)) {
+    Serial.println("Error setting up MDNS responder!");
+  }
+  MDNS.addService("http", "tcp", 80);
+}
+
 String getContentType(String filename)
 { // convert the file extension to the MIME type
   if (filename.endsWith(".html"))
@@ -113,7 +120,8 @@ void setupWebserver()
   server.begin();
 }
 
-void messageReceived(String &topic, String &payload) {
+void messageReceived(String &topic, String &payload)
+{
   Serial.println("incoming: " + topic + " - " + payload);
 }
 
@@ -139,6 +147,7 @@ void setupMQTT()
   if (mqttServer.fromString(MQTT_HOST))
   {
     mqttClient.setServer(mqttServer, 1883);
+    mqttClient.setBufferSize(512);
   }
   else
   {
@@ -150,44 +159,43 @@ void setupMQTT()
 void setupOTA()
 {
   ArduinoOTA.setHostname(HOSTNAME);
-  ArduinoOTA.onStart([]() {
-    String type;
-    if (ArduinoOTA.getCommand() == U_FLASH)
-    {
-      type = "sketch";
-    }
-    else
-    { // U_SPIFFS
-      type = "filesystem";
-      LittleFS.end();
-    }
-    Serial.println("Start updating " + type);
-  });
-  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
-    Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
-  });
-  ArduinoOTA.onEnd([]() {
-    Serial.println("\nEnd");
-  });
-  ArduinoOTA.onError([](ota_error_t error) {
-    Serial.printf("Error[%u]: ", error);
-    if (error == OTA_AUTH_ERROR)
-      Serial.println("Auth Failed");
-    else if (error == OTA_BEGIN_ERROR)
-      Serial.println("Begin Failed");
-    else if (error == OTA_CONNECT_ERROR)
-      Serial.println("Connect Failed");
-    else if (error == OTA_RECEIVE_ERROR)
-      Serial.println("Receive Failed");
-    else if (error == OTA_END_ERROR)
-      Serial.println("End Failed");
-  });
+  ArduinoOTA.onStart([]()
+                     {
+                       String type;
+                       if (ArduinoOTA.getCommand() == U_FLASH)
+                       {
+                         type = "sketch";
+                       }
+                       else
+                       { // U_SPIFFS
+                         type = "filesystem";
+                         LittleFS.end();
+                       }
+                       Serial.println("Start updating " + type);
+                     });
+  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total)
+                        { Serial.printf("Progress: %u%%\r", (progress / (total / 100))); });
+  ArduinoOTA.onEnd([]()
+                   { Serial.println("\nEnd"); });
+  ArduinoOTA.onError([](ota_error_t error)
+                     {
+                       Serial.printf("Error[%u]: ", error);
+                       if (error == OTA_AUTH_ERROR)
+                         Serial.println("Auth Failed");
+                       else if (error == OTA_BEGIN_ERROR)
+                         Serial.println("Begin Failed");
+                       else if (error == OTA_CONNECT_ERROR)
+                         Serial.println("Connect Failed");
+                       else if (error == OTA_RECEIVE_ERROR)
+                         Serial.println("Receive Failed");
+                       else if (error == OTA_END_ERROR)
+                         Serial.println("End Failed");
+                     });
   ArduinoOTA.begin();
 }
 
 void setupPms()
 {
-  Serial.println("Initializing PMS, wait 30 seconds for stable readings...");
   swSerial.begin(9600, SWSERIAL_8N1, PMS_RX, PMS_TX);
   pms.init();
 }
@@ -221,8 +229,8 @@ void checkIaqSensorStatus(void)
 
 void setupBme()
 {
+  Wire.begin();
   iaqSensor.begin(BME680_I2C_ADDR_PRIMARY, Wire);
-  logLine("\nBSEC library version " + String(iaqSensor.version.major) + "." + String(iaqSensor.version.minor) + "." + String(iaqSensor.version.major_bugfix) + "." + String(iaqSensor.version.minor_bugfix));
   checkIaqSensorStatus();
   bsec_virtual_sensor_t sensorList[10] = {
       BSEC_OUTPUT_RAW_TEMPERATURE,
@@ -243,7 +251,6 @@ void setupBme()
 
 void setup()
 {
-  Wire.begin();
   Serial.begin(115200);
   delay(10);
   Serial.println();
@@ -253,23 +260,31 @@ void setup()
   setupOTA();
   setupWebserver();
   setupMQTT();
+
+  sensorData = new SimpleMap<String, double>([](String &a, String &b) -> int
+                                             {
+                                               if (a == b)
+                                                 return 0;
+                                               if (a > b)
+                                                 return 1;
+                                               /*if (a < b)*/ return -1;
+                                             });
 }
 
-String readPms()
+void readPms()
 {
   pms.read();
   if (pms)
   { // successfull read
-    Serial.printf("PM <=  1µm: %5d µg/m³\r\n", pms.pm01);
-    Serial.printf("PM <=2.5µm: %5d µg/m³\r\n", pms.pm25);
-    Serial.printf("PM <= 10µm: %5d µg/m³\r\n", pms.pm10);
-    Serial.printf("NC >=0.3µm: %5d #/100cm³\r\n", pms.n0p3);
-    Serial.printf("NC >=0.5µm: %5d #/100cm³\r\n", pms.n0p5);
-    Serial.printf("NC >=  1µm: %5d #/100cm³\r\n", pms.n1p0);
-    Serial.printf("NC >=2.5µm: %5d #/100cm³\r\n", pms.n2p5);
-    Serial.printf("NC >=  5µm: %5d #/100cm³\r\n", pms.n5p0);
-    Serial.printf("NC >= 10µm: %5d #/100cm³\r\n", pms.n10p0);
-    logLine("-----");
+    sensorData->put("pm1", pms.pm01);
+    sensorData->put("pm2.5", pms.pm25);
+    sensorData->put("pm10", pms.pm10);
+    sensorData->put("nc0.3", pms.n0p3);
+    sensorData->put("nc0.5", pms.n0p5);
+    sensorData->put("nc1", pms.n1p0);
+    sensorData->put("nc2.5", pms.n2p5);
+    sensorData->put("nc5", pms.n5p0);
+    sensorData->put("nc10", pms.n10p0);
   }
   else
   {
@@ -303,41 +318,61 @@ String readPms()
       break;
     }
   }
-  String pmData = "pm1=" + String(pms.data[0]) + ",pm2.5=" + String(pms.data[1]) + ",pm10=" + String(pms.data[2]);
-  String ncData = "nc0.3=" + String(pms.data[3]) + ",nc0.5=" + String(pms.data[4]) + ",nc1=" + String(pms.data[5]) + ",nc2.5=" + String(pms.data[6]) + ",nc5=" + String(pms.data[7]) + ",nc10=" + String(pms.data[8]);
-  return pmData + "," + ncData;
 }
 
-String readBme()
+void logPms()
+{
+  Serial.printf("PM <=  1µm: %5d µg/m³\r\n", (int)sensorData->get("pm1"));
+  Serial.printf("PM <=2.5µm: %5d µg/m³\r\n", (int)sensorData->get("pm2.5"));
+  Serial.printf("PM <= 10µm: %5d µg/m³\r\n", (int)sensorData->get("pm10"));
+  Serial.printf("NC >=0.3µm: %5d #/100cm³\r\n", (int)sensorData->get("nc0.3"));
+  Serial.printf("NC >=0.5µm: %5d #/100cm³\r\n", (int)sensorData->get("nc0.5"));
+  Serial.printf("NC >=  1µm: %5d #/100cm³\r\n", (int)sensorData->get("nc1"));
+  Serial.printf("NC >=2.5µm: %5d #/100cm³\r\n", (int)sensorData->get("nc2.5"));
+  Serial.printf("NC >=  5µm: %5d #/100cm³\r\n", (int)sensorData->get("nc5"));
+  Serial.printf("NC >= 10µm: %5d #/100cm³\r\n", (int)sensorData->get("nc10"));
+}
+
+void readBme()
 {
   if (iaqSensor.run())
   { // If new data is available
-    Serial.printf("Temperature  : %f °C\r\n", iaqSensor.temperature);
-    Serial.printf("Humidity     : %f %%\r\n", iaqSensor.humidity);
-    Serial.printf("Pressure     : %f hPa\r\n", iaqSensor.pressure);
-    Serial.printf("IAQ          : %f\r\n", iaqSensor.iaq);
-    Serial.printf("IAQ Accuracy : %d\r\n", iaqSensor.iaqAccuracy);
-    Serial.printf("Static IAQ   : %f\r\n", iaqSensor.staticIaq);
-    Serial.printf("Stat IAQ Acc : %d\r\n", iaqSensor.staticIaqAccuracy);
-    Serial.printf("CO2 Equiv    : %f\r\n", iaqSensor.co2Equivalent);
-    Serial.printf("CO2 Accuracy : %d\r\n", iaqSensor.co2Accuracy);
-    Serial.printf("bVOC Equiv   : %f\r\n", iaqSensor.breathVocEquivalent);
-    Serial.printf("bVOC Accuracy: %d\r\n", iaqSensor.breathVocAccuracy);
-    Serial.printf("Gas Percent  : %f %%\r\n", iaqSensor.gasPercentage);
-    Serial.printf("Gas Per Accur: %d\r\n", iaqSensor.gasPercentageAcccuracy);
-    Serial.printf("Raw Temp     : %f °C\r\n", iaqSensor.rawTemperature);
-    Serial.printf("Raw Rel Humid: %f %%\r\n", iaqSensor.rawHumidity);
-    Serial.printf("Gas Resist   : %f Ohm\r\n", iaqSensor.gasResistance);
+    sensorData->put("temperature", iaqSensor.temperature);
+    sensorData->put("humidity", iaqSensor.humidity);
+    sensorData->put("pressure", iaqSensor.pressure);
+    sensorData->put("iaq", iaqSensor.iaq);
+    sensorData->put("iaq_acc", iaqSensor.iaqAccuracy);
+    sensorData->put("siaq", iaqSensor.staticIaq);
+    sensorData->put("siaq_acc", iaqSensor.staticIaqAccuracy);
+    sensorData->put("eco2", iaqSensor.co2Equivalent);
+    sensorData->put("eco2_acc", iaqSensor.co2Accuracy);
+    sensorData->put("bvoc", iaqSensor.breathVocEquivalent);
+    sensorData->put("bvoc_acc", iaqSensor.breathVocAccuracy);
   }
   else
   {
     checkIaqSensorStatus();
   }
-  return "temperature=" + String(iaqSensor.temperature) + ",humidity=" + String(iaqSensor.humidity) + ",pressure=" + String(iaqSensor.pressure) +
-         ",iaq=" + String(iaqSensor.iaq) + ",iaq_acc=" + String(iaqSensor.iaqAccuracy) +
-         ",s_iaq=" + String(iaqSensor.staticIaq) + ",s_iaq_acc=" + String(iaqSensor.staticIaqAccuracy) +
-         ",eco2=" + String(iaqSensor.co2Equivalent) + ",eco2_acc=" + String(iaqSensor.co2Accuracy) +
-         ",bvoc=" + String(iaqSensor.breathVocEquivalent) + ",bvoc_acc=" + String(iaqSensor.breathVocAccuracy);
+}
+
+void logBme()
+{
+  Serial.printf("Temperature  : %.2f °C\r\n", sensorData->get("temperature"));
+  Serial.printf("Humidity     : %.2f %%\r\n", sensorData->get("humidity"));
+  Serial.printf("Pressure     : %d hPa\r\n", (int)sensorData->get("pressure"));
+  Serial.printf("IAQ          : %.2f\r\n", sensorData->get("iaq"));
+  Serial.printf("IAQ Accuracy : %d\r\n", (int)sensorData->get("iaq_acc"));
+  Serial.printf("Static IAQ   : %.2f\r\n", sensorData->get("siaq"));
+  Serial.printf("Stat IAQ Acc : %d\r\n", (int)sensorData->get("siaq_acc"));
+  Serial.printf("CO2 Equiv    : %.2f ppm\r\n", sensorData->get("eco2"));
+  Serial.printf("CO2 Accuracy : %d\r\n", (int)sensorData->get("eco2_acc"));
+  Serial.printf("bVOC Equiv   : %.2f ppm\r\n", sensorData->get("bvoc"));
+  Serial.printf("bVOC Accuracy: %d\r\n", (int)sensorData->get("bvoc_acc"));
+  //Serial.printf("Gas Percent  : %f %%\r\n", iaqSensor.gasPercentage);
+  //Serial.printf("Gas Per Accur: %d\r\n", iaqSensor.gasPercentageAcccuracy);
+  //Serial.printf("Raw Temp     : %f °C\r\n", iaqSensor.rawTemperature);
+  //Serial.printf("Raw Rel Humid: %f %%\r\n", iaqSensor.rawHumidity);
+  //Serial.printf("Gas Resist   : %f Ohm\r\n", iaqSensor.gasResistance);
 }
 
 void publishMetric(String metricValues)
@@ -350,42 +385,67 @@ void publishMetric(String metricValues)
   mqttClient.publish(MQTT_TOPIC, payload);
 }
 
-void getSensorData()
+void readSensorData()
 {
+  readPms();
+  readBme();
+}
 
-  String pmsData = readPms();
-  String bmeData = readBme();
-  String metricValues = pmsData + "," + bmeData;
+void sendSensorData()
+{
+  String metricValues;
+  for (int i = 0; i < sensorData->size(); i++)
+  {
+    metricValues += sensorData->getKey(i) + "=" + sensorData->getData(i);
+    if (i < sensorData->size() - 1)
+    {
+      metricValues += ",";
+    }
+  }
   publishMetric(metricValues);
-  logLine("#####");
+}
+
+void logSensorData()
+{
+  logLine("\n----- PMS5003 -----");
+  logPms();
+  logLine("\n----- BME680 -----");
+  logBme();
+  logLine("\n#####");
 }
 
 void loop()
 {
   ArduinoOTA.handle();
   server.handleClient();
-  mdns.update();
+  MDNS.update();
 
-  unsigned long currentMillis = millis();
-  if (currentMillis - checkMillis > checkInterval)
+  static unsigned long readTimer = millis();
+  if (millis() - readTimer >= 1000)
+  {
+    readSensorData();
+    readTimer = millis();
+  }
+
+  static unsigned long sendTimer = millis();
+  if (millis() - sendTimer >= 5000)
   {
     if ((WiFi.status() != WL_CONNECTED))
     {
+      logLine("Wifi not connected, reconnecting...");
       WiFi.disconnect();
       WiFi.reconnect();
     }
     if (!mqttClient.connected())
     {
+      logLine("MQTT not connected, reconnecting...");
       connectMQTT();
     }
-    checkMillis = currentMillis;
-  }
-  mqttClient.loop();
 
-  static unsigned long updateTimer = millis();
-  if (millis() - updateTimer >= 5000)
-  {
-    updateTimer = millis();
-    getSensorData();
+    sendSensorData();
+    logSensorData();
+    sendTimer = millis();
   }
+
+  mqttClient.loop();
 }