diff --git a/Code/Arduino/AmsToMqttBridge/AmsToMqttBridge.ino b/Code/Arduino/AmsToMqttBridge/AmsToMqttBridge.ino
index b20d2b75..dbb48bb8 100644
--- a/Code/Arduino/AmsToMqttBridge/AmsToMqttBridge.ino
+++ b/Code/Arduino/AmsToMqttBridge/AmsToMqttBridge.ino
@@ -9,353 +9,408 @@
 #include <OneWire.h>
 #include <ArduinoJson.h>
 #include <PubSubClient.h>
-#include "configuration.h"
-#include "accesspoint.h"
 #include <HanReader.h>
 #include <Kaifa.h>
 #include <Kamstrup.h>
+#include "configuration.h"
+#include "accesspoint.h"
 
 #define WIFI_CONNECTION_TIMEOUT 30000;
 #define TEMP_SENSOR_PIN 5 // Temperature sensor connected to GPIO5
+#define LED_PIN 2 // The blue on-board LED of the ESP
 
 OneWire oneWire(TEMP_SENSOR_PIN);
 DallasTemperature tempSensor(&oneWire);
 long lastTempDebug = 0;
 
+// Object used to boot as Access Point
 accesspoint ap;
+
+// WiFi client and MQTT client
 WiFiClient *client;
 PubSubClient mqtt;
-HardwareSerial* debugger;
 
-// The HAN Port reader
+// Object used for debugging
+HardwareSerial* debugger = NULL;
+
+// The HAN Port reader, used to read serial data and decode DLMS
 HanReader hanReader;
 
 // the setup function runs once when you press reset or power the board
 void setup() 
 {
+	// Uncomment to debug over the same port as used for HAN communication
 	debugger = &Serial;
 	
-	// Setup serial port for debugging
-	debugger->begin(2400, SERIAL_8E1);
-	while (!&debugger);
-	debugger->println("Started...");
-	
+	if (debugger) {
+		// Setup serial port for debugging
+		debugger->begin(2400, SERIAL_8E1);
+		while (!&debugger);
+		debugger->println("Started...");
+	}
+
 	// Assign pin for boot as AP
 	delay(1000);
 	pinMode(0, INPUT_PULLUP);
 	
 	// Flash the blue LED, to indicate we can boot as AP now
-	pinMode(2, OUTPUT);
-	digitalWrite(2, LOW);
+	pinMode(LED_PIN, OUTPUT);
+	digitalWrite(LED_PIN, LOW);
 	
 	// Initialize the AP
 	ap.setup(0, Serial);
 	
 	// Turn off the blue LED
-	digitalWrite(2, HIGH);
+	digitalWrite(LED_PIN, HIGH);
 
 	if (!ap.isActivated)
 	{
 		setupWiFi();
 		hanReader.setup(&Serial, 2400, SERIAL_8E1, debugger);
-		hanReader.compensateFor09HeaderBug = (ap.config.meterType == 1); // To compensate for the known Kaifa bug
+		
+		// Compensate for the known Kaifa bug
+		hanReader.compensateFor09HeaderBug = (ap.config.meterType == 1);
 	}
 }
 
-void setupWiFi()
-{
-	if (ap.config.isSecure())
-		client = new WiFiClientSecure();
-	else
-		client = new WiFiClient();
-
-	mqtt = PubSubClient(*client);
-
-	WiFi.enableAP(false);
-	WiFi.begin(ap.config.ssid, ap.config.ssidPassword);
-	mqtt.setServer(ap.config.mqtt, ap.config.mqttPort);
-
-	//std::function<void(char*, unsigned char*, unsigned int)> vCallback = MakeDelegate(this, xnsClientClass::mqttMessageReceived);
-	mqtt.setCallback(mqttMessageReceived);
-	MQTT_connect();
-
-	sendMqttData("Connected!");
-}
-
-void mqttMessageReceived(char* topic, unsigned char* payload, unsigned int length)
-{
-	// make it a null-terminated string
-	char message[1000];
-	for (int i = 0; i < length; i++)
-		message[i] = payload[i];
-	message[length] = 0;
-
-	debugger->println("Incoming MQTT message:");
-	debugger->print("[");
-	debugger->print(topic);
-	debugger->print("] ");
-	debugger->println(message);
-}
-
 // the loop function runs over and over again until power down or reset
-void loop() 
+void loop()
 {
-	// Only do normal stupp if we're not booted as AP
+	// Only do normal stuff if we're not booted as AP
 	if (!ap.loop())
 	{
+		// turn off the blue LED
+		digitalWrite(LED_PIN, HIGH);
+
+		// allow the MQTT client some resources
 		mqtt.loop();
 		delay(10); // <- fixes some issues with WiFi stability
 
+		// Reconnect to WiFi and MQTT as needed
 		if (!mqtt.connected()) {
 			MQTT_connect();
 		}
 		else
 		{
-			debugTemperature();
+			// Read data from the HAN port
 			readHanPort();
 		}
 	}
-}
-void debugTemperature()
-{
-	if (lastTempDebug + 5000 < millis())
+	else
 	{
-		lastTempDebug = millis();
-		tempSensor.requestTemperatures();
-		float temperature = tempSensor.getTempCByIndex(0);
-		debugger->print("Temperature is ");
-		debugger->print(temperature);
-		debugger->println(" degrees");
-	}
-}
-void readHanPort()
-{
-	switch (ap.config.meterType)
-	{
-	case 1: // Kaifa
-		readHanPort_Kaifa();
-		break;
-	case 2: // Kamstrup
-		readHanPort_Kamstrup();
-		break;
-	case 3: // Aidon
-		readHanPort_Aidon();
-		break;
-	default:
-		debugger->print("Meter type ");
-		debugger->print(ap.config.meterType, HEX);
-		debugger->println(" is unknown");
-		delay(10000);
-		break;
+		// Continously flash the blue LED when AP mode
+		if (millis() / 1000 % 2 == 0)
+			digitalWrite(LED_PIN, LOW);
+		else
+			digitalWrite(LED_PIN, HIGH);
 	}
 }
 
-void readHanPort_Aidon()
+void setupWiFi()
 {
-	debugger->println("Meter type Aidon is not yet implemented");
+	// Turn off AP
+	WiFi.enableAP(false);
+	
+	// Connect to WiFi
+	WiFi.begin(ap.config.ssid, ap.config.ssidPassword);
+	
+	// Initialize WiFi and MQTT clients
+	if (ap.config.isSecure())
+		client = new WiFiClientSecure();
+	else
+		client = new WiFiClient();
+	mqtt = PubSubClient(*client);
+	mqtt.setServer(ap.config.mqtt, ap.config.mqttPort);
+
+	// Direct incoming MQTT messages
+	if (ap.config.mqttSubscribeTopic != 0 && strlen(ap.config.mqttSubscribeTopic) > 0)
+		mqtt.setCallback(mqttMessageReceived);
+
+	// Connect to the MQTT server
+	MQTT_connect();
+
+	// Notify everyone we're here!
+	sendMqttData("Connected!");
+}
+
+void mqttMessageReceived(char* topic, unsigned char* payload, unsigned int length)
+{
+	// make the incoming message a null-terminated string
+	char message[1000];
+	for (int i = 0; i < length; i++)
+		message[i] = payload[i];
+	message[length] = 0;
+
+	if (debugger) {
+		debugger->println("Incoming MQTT message:");
+		debugger->print("[");
+		debugger->print(topic);
+		debugger->print("] ");
+		debugger->println(message);
+	}
+
+	// Do whatever needed here...
+	// Ideas could be to query for values or to initiate OTA firmware update
+}
+
+void readHanPort()
+{
+	if (hanReader.read())
+	{
+		// Flash LED on, this shows us that data is received
+		digitalWrite(LED_PIN, LOW);
+
+		// Get the list identifier
+		int listSize = hanReader.getListSize();
+
+		switch (ap.config.meterType)
+		{
+		case 1: // Kaifa
+			readHanPort_Kaifa(listSize);
+			break;
+		case 2: // Kamstrup
+			readHanPort_Kamstrup(listSize);
+			break;
+		case 3: // Aidon
+			readHanPort_Aidon(listSize);
+			break;
+		default:
+			debugger->print("Meter type ");
+			debugger->print(ap.config.meterType, HEX);
+			debugger->println(" is unknown");
+			delay(10000);
+			break;
+		}
+
+		// Flash LED off
+		digitalWrite(LED_PIN, HIGH);
+	}
+}
+
+void readHanPort_Aidon(int listSize)
+{
+	if (debugger)
+		debugger->println("Meter type Aidon is not yet implemented");
 	delay(10000);
 }
 
-void readHanPort_Kamstrup()
+void readHanPort_Kamstrup(int listSize)
 {
-	if (hanReader.read())
+	// Only care for the ACtive Power Imported, which is found in the first list
+	if (listSize == (int)Kamstrup::List1 || listSize == (int)Kamstrup::List2)
 	{
-		// Get the list identifier
-		int listSize = hanReader.getListSize();
-
-		// Only care for the ACtive Power Imported, which is found in the first list
-		if (listSize == (int)Kamstrup::List1 || listSize == (int)Kamstrup::List2)
+		if (listSize == (int)Kamstrup::List1)
 		{
-			if (listSize == (int)Kamstrup::List1)
-			{
-				String id = hanReader.getString((int)Kamstrup_List1::ListVersionIdentifier);
-				if (debugger) debugger->println(id);
-			}
-			else if (listSize == (int)Kamstrup::List2)
-			{
-				String id = hanReader.getString((int)Kamstrup_List2::ListVersionIdentifier);
-				if (debugger) debugger->println(id);
-			}
-
-			// Get the timestamp (as unix time) from the package
-			time_t time = hanReader.getPackageTime();
-			if (debugger) debugger->print("Time of the package is: ");
-			if (debugger) debugger->println(time);
-
-			// Define a json object to keep the data
-			StaticJsonBuffer<500> jsonBuffer;
-			JsonObject& root = jsonBuffer.createObject();
-
-			// Any generic useful info here
-			root["id"] = WiFi.macAddress();
-			root["up"] = millis();
-			root["t"] = time;
-
-			// Add a sub-structure to the json object, 
-			// to keep the data from the meter itself
-			JsonObject& data = root.createNestedObject("data");
-
-			// Get the temperature too
-			tempSensor.requestTemperatures();
-			float temperature = tempSensor.getTempCByIndex(0);
-			data["temp"] = temperature;
-
-			// Based on the list number, get all details 
-			// according to OBIS specifications for the meter
-			if (listSize == (int)Kamstrup::List1)
-			{
-				data["lv"] = hanReader.getString((int)Kamstrup_List1::ListVersionIdentifier);
-				data["id"] = hanReader.getString((int)Kamstrup_List1::MeterID);
-				data["type"] = hanReader.getString((int)Kamstrup_List1::MeterType);
-				data["P"] = hanReader.getInt((int)Kamstrup_List1::ActiveImportPower);
-				data["Q"] = hanReader.getInt((int)Kamstrup_List1::ReactiveImportPower);
-				data["I1"] = hanReader.getInt((int)Kamstrup_List1::CurrentL1);
-				data["I2"] = hanReader.getInt((int)Kamstrup_List1::CurrentL2);
-				data["I3"] = hanReader.getInt((int)Kamstrup_List1::CurrentL3);
-				data["U1"] = hanReader.getInt((int)Kamstrup_List1::VoltageL1);
-				data["U2"] = hanReader.getInt((int)Kamstrup_List1::VoltageL2);
-				data["U3"] = hanReader.getInt((int)Kamstrup_List1::VoltageL3);
-			}
-			else if (listSize == (int)Kamstrup::List2)
-			{
-				data["lv"] = hanReader.getString((int)Kamstrup_List2::ListVersionIdentifier);;
-				data["id"] = hanReader.getString((int)Kamstrup_List2::MeterID);
-				data["type"] = hanReader.getString((int)Kamstrup_List2::MeterType);
-				data["P"] = hanReader.getInt((int)Kamstrup_List2::ActiveImportPower);
-				data["Q"] = hanReader.getInt((int)Kamstrup_List2::ReactiveImportPower);
-				data["I1"] = hanReader.getInt((int)Kamstrup_List2::CurrentL1);
-				data["I2"] = hanReader.getInt((int)Kamstrup_List2::CurrentL2);
-				data["I3"] = hanReader.getInt((int)Kamstrup_List2::CurrentL3);
-				data["U1"] = hanReader.getInt((int)Kamstrup_List2::VoltageL1);
-				data["U2"] = hanReader.getInt((int)Kamstrup_List2::VoltageL2);
-				data["U3"] = hanReader.getInt((int)Kamstrup_List2::VoltageL3);
-				data["tPI"] = hanReader.getInt((int)Kamstrup_List2::CumulativeActiveImportEnergy);
-				data["tPO"] = hanReader.getInt((int)Kamstrup_List2::CumulativeActiveExportEnergy);
-				data["tQI"] = hanReader.getInt((int)Kamstrup_List2::CumulativeReactiveImportEnergy);
-				data["tQO"] = hanReader.getInt((int)Kamstrup_List2::CumulativeReactiveExportEnergy);
-			}
-
-			// Write the json to the debug port
-			root.printTo(Serial1);
-			Serial1.println();
-
-			// Publish the json to the MQTT server
-			char msg[1024];
-			root.printTo(msg, 1024);
-			mqtt.publish(ap.config.mqttPublishTopic, msg);
+			String id = hanReader.getString((int)Kamstrup_List1::ListVersionIdentifier);
+			if (debugger) debugger->println(id);
 		}
+		else if (listSize == (int)Kamstrup::List2)
+		{
+			String id = hanReader.getString((int)Kamstrup_List2::ListVersionIdentifier);
+			if (debugger) debugger->println(id);
+		}
+
+		// Get the timestamp (as unix time) from the package
+		time_t time = hanReader.getPackageTime();
+		if (debugger) debugger->print("Time of the package is: ");
+		if (debugger) debugger->println(time);
+
+		// Define a json object to keep the data
+		StaticJsonBuffer<500> jsonBuffer;
+		JsonObject& root = jsonBuffer.createObject();
+
+		// Any generic useful info here
+		root["id"] = WiFi.macAddress();
+		root["up"] = millis();
+		root["t"] = time;
+
+		// Add a sub-structure to the json object, 
+		// to keep the data from the meter itself
+		JsonObject& data = root.createNestedObject("data");
+
+		// Get the temperature too
+		tempSensor.requestTemperatures();
+		float temperature = tempSensor.getTempCByIndex(0);
+		data["temp"] = temperature;
+
+		// Based on the list number, get all details 
+		// according to OBIS specifications for the meter
+		if (listSize == (int)Kamstrup::List1)
+		{
+			data["lv"] = hanReader.getString((int)Kamstrup_List1::ListVersionIdentifier);
+			data["id"] = hanReader.getString((int)Kamstrup_List1::MeterID);
+			data["type"] = hanReader.getString((int)Kamstrup_List1::MeterType);
+			data["P"] = hanReader.getInt((int)Kamstrup_List1::ActiveImportPower);
+			data["Q"] = hanReader.getInt((int)Kamstrup_List1::ReactiveImportPower);
+			data["I1"] = hanReader.getInt((int)Kamstrup_List1::CurrentL1);
+			data["I2"] = hanReader.getInt((int)Kamstrup_List1::CurrentL2);
+			data["I3"] = hanReader.getInt((int)Kamstrup_List1::CurrentL3);
+			data["U1"] = hanReader.getInt((int)Kamstrup_List1::VoltageL1);
+			data["U2"] = hanReader.getInt((int)Kamstrup_List1::VoltageL2);
+			data["U3"] = hanReader.getInt((int)Kamstrup_List1::VoltageL3);
+		}
+		else if (listSize == (int)Kamstrup::List2)
+		{
+			data["lv"] = hanReader.getString((int)Kamstrup_List2::ListVersionIdentifier);;
+			data["id"] = hanReader.getString((int)Kamstrup_List2::MeterID);
+			data["type"] = hanReader.getString((int)Kamstrup_List2::MeterType);
+			data["P"] = hanReader.getInt((int)Kamstrup_List2::ActiveImportPower);
+			data["Q"] = hanReader.getInt((int)Kamstrup_List2::ReactiveImportPower);
+			data["I1"] = hanReader.getInt((int)Kamstrup_List2::CurrentL1);
+			data["I2"] = hanReader.getInt((int)Kamstrup_List2::CurrentL2);
+			data["I3"] = hanReader.getInt((int)Kamstrup_List2::CurrentL3);
+			data["U1"] = hanReader.getInt((int)Kamstrup_List2::VoltageL1);
+			data["U2"] = hanReader.getInt((int)Kamstrup_List2::VoltageL2);
+			data["U3"] = hanReader.getInt((int)Kamstrup_List2::VoltageL3);
+			data["tPI"] = hanReader.getInt((int)Kamstrup_List2::CumulativeActiveImportEnergy);
+			data["tPO"] = hanReader.getInt((int)Kamstrup_List2::CumulativeActiveExportEnergy);
+			data["tQI"] = hanReader.getInt((int)Kamstrup_List2::CumulativeReactiveImportEnergy);
+			data["tQO"] = hanReader.getInt((int)Kamstrup_List2::CumulativeReactiveExportEnergy);
+		}
+
+		// Write the json to the debug port
+		if (debugger) {
+			debugger->print("Sending data to MQTT: ");
+			root.printTo(*debugger);
+			debugger->println();
+		}
+
+		// Make sure we have configured a publish topic
+		if (ap.config.mqttPublishTopic == 0 || strlen(ap.config.mqttPublishTopic) == 0)
+			return;
+
+		// Publish the json to the MQTT server
+		char msg[1024];
+		root.printTo(msg, 1024);
+		mqtt.publish(ap.config.mqttPublishTopic, msg);
 	}
 }
 
 
-void readHanPort_Kaifa() {
-	if (hanReader.read())
+void readHanPort_Kaifa(int listSize) 
+{
+	// Only care for the ACtive Power Imported, which is found in the first list
+	if (listSize == (int)Kaifa::List1 || listSize == (int)Kaifa::List2 || listSize == (int)Kaifa::List3)
 	{
-		// Get the list identifier
-		int listSize = hanReader.getListSize();
-
-		// Only care for the ACtive Power Imported, which is found in the first list
-		if (listSize == (int)Kaifa::List1 || listSize == (int)Kaifa::List2 || listSize == (int)Kaifa::List3)
+		if (listSize == (int)Kaifa::List1)
 		{
-			if (listSize == (int)Kaifa::List1)
-			{
-				if (debugger) debugger->println(" (list #1 has no ID)");
-			}
-			else
-			{
-				String id = hanReader.getString((int)Kaifa_List2::ListVersionIdentifier);
-				if (debugger) debugger->println(id);
-			}
-
-			// Get the timestamp (as unix time) from the package
-			time_t time = hanReader.getPackageTime();
-			if (debugger) debugger->print("Time of the package is: ");
-			if (debugger) debugger->println(time);
-
-			// Define a json object to keep the data
-			StaticJsonBuffer<500> jsonBuffer;
-			JsonObject& root = jsonBuffer.createObject();
-
-			// Any generic useful info here
-			root["id"] = WiFi.macAddress();
-			root["up"] = millis();
-			root["t"] = time;
-
-			// Add a sub-structure to the json object, 
-			// to keep the data from the meter itself
-			JsonObject& data = root.createNestedObject("data");
-
-			// Get the temperature too
-			tempSensor.requestTemperatures();
-			float temperature = tempSensor.getTempCByIndex(0);
-			data["temp"] = temperature;
-
-			// Based on the list number, get all details 
-			// according to OBIS specifications for the meter
-			if (listSize == (int)Kaifa::List1)
-			{
-				data["P"] = hanReader.getInt((int)Kaifa_List1::ActivePowerImported);
-			}
-			else if (listSize == (int)Kaifa::List2)
-			{
-				data["lv"] = hanReader.getString((int)Kaifa_List2::ListVersionIdentifier);
-				data["id"] = hanReader.getString((int)Kaifa_List2::MeterID);
-				data["type"] = hanReader.getString((int)Kaifa_List2::MeterType);
-				data["P"] = hanReader.getInt((int)Kaifa_List2::ActiveImportPower);
-				data["Q"] = hanReader.getInt((int)Kaifa_List2::ReactiveImportPower);
-				data["I1"] = hanReader.getInt((int)Kaifa_List2::CurrentL1);
-				data["I2"] = hanReader.getInt((int)Kaifa_List2::CurrentL2);
-				data["I3"] = hanReader.getInt((int)Kaifa_List2::CurrentL3);
-				data["U1"] = hanReader.getInt((int)Kaifa_List2::VoltageL1);
-				data["U2"] = hanReader.getInt((int)Kaifa_List2::VoltageL2);
-				data["U3"] = hanReader.getInt((int)Kaifa_List2::VoltageL3);
-			}
-			else if (listSize == (int)Kaifa::List3)
-			{
-				data["lv"] = hanReader.getString((int)Kaifa_List3::ListVersionIdentifier);;
-				data["id"] = hanReader.getString((int)Kaifa_List3::MeterID);
-				data["type"] = hanReader.getString((int)Kaifa_List3::MeterType);
-				data["P"] = hanReader.getInt((int)Kaifa_List3::ActiveImportPower);
-				data["Q"] = hanReader.getInt((int)Kaifa_List3::ReactiveImportPower);
-				data["I1"] = hanReader.getInt((int)Kaifa_List3::CurrentL1);
-				data["I2"] = hanReader.getInt((int)Kaifa_List3::CurrentL2);
-				data["I3"] = hanReader.getInt((int)Kaifa_List3::CurrentL3);
-				data["U1"] = hanReader.getInt((int)Kaifa_List3::VoltageL1);
-				data["U2"] = hanReader.getInt((int)Kaifa_List3::VoltageL2);
-				data["U3"] = hanReader.getInt((int)Kaifa_List3::VoltageL3);
-				data["tPI"] = hanReader.getInt((int)Kaifa_List3::CumulativeActiveImportEnergy);
-				data["tPO"] = hanReader.getInt((int)Kaifa_List3::CumulativeActiveExportEnergy);
-				data["tQI"] = hanReader.getInt((int)Kaifa_List3::CumulativeReactiveImportEnergy);
-				data["tQO"] = hanReader.getInt((int)Kaifa_List3::CumulativeReactiveExportEnergy);
-			}
-
-			// Write the json to the debug port
-			root.printTo(Serial1);
-			Serial1.println();
-
-			// Publish the json to the MQTT server
-			char msg[1024];
-			root.printTo(msg, 1024);
-			mqtt.publish("sensors/out/espdebugger", msg);
+			if (debugger) debugger->println(" (list #1 has no ID)");
 		}
+		else
+		{
+			String id = hanReader.getString((int)Kaifa_List2::ListVersionIdentifier);
+			if (debugger) debugger->println(id);
+		}
+
+		// Get the timestamp (as unix time) from the package
+		time_t time = hanReader.getPackageTime();
+		if (debugger) debugger->print("Time of the package is: ");
+		if (debugger) debugger->println(time);
+
+		// Define a json object to keep the data
+		//StaticJsonBuffer<500> jsonBuffer;
+		DynamicJsonBuffer jsonBuffer;
+		JsonObject& root = jsonBuffer.createObject();
+
+		// Any generic useful info here
+		root["id"] = WiFi.macAddress();
+		root["up"] = millis();
+		root["t"] = time;
+
+		// Add a sub-structure to the json object, 
+		// to keep the data from the meter itself
+		JsonObject& data = root.createNestedObject("data");
+
+		// Get the temperature too
+		tempSensor.requestTemperatures();
+		float temperature = tempSensor.getTempCByIndex(0);
+		data["temp"] = String(temperature);
+
+		// Based on the list number, get all details 
+		// according to OBIS specifications for the meter
+		if (listSize == (int)Kaifa::List1)
+		{
+			data["P"] = hanReader.getInt((int)Kaifa_List1::ActivePowerImported);
+		}
+		else if (listSize == (int)Kaifa::List2)
+		{
+			data["lv"] = hanReader.getString((int)Kaifa_List2::ListVersionIdentifier);
+			data["id"] = hanReader.getString((int)Kaifa_List2::MeterID);
+			data["type"] = hanReader.getString((int)Kaifa_List2::MeterType);
+			data["P"] = hanReader.getInt((int)Kaifa_List2::ActiveImportPower);
+			data["Q"] = hanReader.getInt((int)Kaifa_List2::ReactiveImportPower);
+			data["I1"] = hanReader.getInt((int)Kaifa_List2::CurrentL1);
+			data["I2"] = hanReader.getInt((int)Kaifa_List2::CurrentL2);
+			data["I3"] = hanReader.getInt((int)Kaifa_List2::CurrentL3);
+			data["U1"] = hanReader.getInt((int)Kaifa_List2::VoltageL1);
+			data["U2"] = hanReader.getInt((int)Kaifa_List2::VoltageL2);
+			data["U3"] = hanReader.getInt((int)Kaifa_List2::VoltageL3);
+		}
+		else if (listSize == (int)Kaifa::List3)
+		{
+			data["lv"] = hanReader.getString((int)Kaifa_List3::ListVersionIdentifier);;
+			data["id"] = hanReader.getString((int)Kaifa_List3::MeterID);
+			data["type"] = hanReader.getString((int)Kaifa_List3::MeterType);
+			data["P"] = hanReader.getInt((int)Kaifa_List3::ActiveImportPower);
+			data["Q"] = hanReader.getInt((int)Kaifa_List3::ReactiveImportPower);
+			data["I1"] = hanReader.getInt((int)Kaifa_List3::CurrentL1);
+			data["I2"] = hanReader.getInt((int)Kaifa_List3::CurrentL2);
+			data["I3"] = hanReader.getInt((int)Kaifa_List3::CurrentL3);
+			data["U1"] = hanReader.getInt((int)Kaifa_List3::VoltageL1);
+			data["U2"] = hanReader.getInt((int)Kaifa_List3::VoltageL2);
+			data["U3"] = hanReader.getInt((int)Kaifa_List3::VoltageL3);
+			data["tPI"] = hanReader.getInt((int)Kaifa_List3::CumulativeActiveImportEnergy);
+			data["tPO"] = hanReader.getInt((int)Kaifa_List3::CumulativeActiveExportEnergy);
+			data["tQI"] = hanReader.getInt((int)Kaifa_List3::CumulativeReactiveImportEnergy);
+			data["tQO"] = hanReader.getInt((int)Kaifa_List3::CumulativeReactiveExportEnergy);
+		}
+
+		// Write the json to the debug port
+		if (debugger) {
+			debugger->print("Sending data to MQTT: ");
+			root.printTo(*debugger);
+			debugger->println();
+		}
+
+		// Make sure we have configured a publish topic
+		if (ap.config.mqttPublishTopic == 0 || strlen(ap.config.mqttPublishTopic) == 0)
+			return;
+
+		// Publish the json to the MQTT server
+		char msg[1024];
+		root.printTo(msg, 1024);
+		mqtt.publish(ap.config.mqttPublishTopic, msg);
 	}
 }
 
-
-
-
 // Function to connect and reconnect as necessary to the MQTT server.
 // Should be called in the loop function and it will take care if connecting.
-void MQTT_connect() {
+void MQTT_connect() 
+{
 	// Connect to WiFi access point.
-	if (debugger) debugger->println(); if (debugger) debugger->println();
-	if (debugger) debugger->print("Connecting to WiFi network ");
-	if (debugger) debugger->println(ap.config.ssid);
+	if (debugger)
+	{
+		debugger->println(); 
+		debugger->println();
+		debugger->print("Connecting to WiFi network ");
+		debugger->println(ap.config.ssid);
+	}
 
+	if (WiFi.status() != WL_CONNECTED)
+	{
+		// Make one first attempt at connect, this seems to considerably speed up the first connection
+		WiFi.disconnect();
+		WiFi.begin(ap.config.ssid, ap.config.ssidPassword);
+		delay(1000);
+	}
+
+	// Wait for the WiFi connection to complete
 	long vTimeout = millis() + WIFI_CONNECTION_TIMEOUT;
 	while (WiFi.status() != WL_CONNECTED) {
 		delay(50);
 		if (debugger) debugger->print(".");
+		
+		// If we timed out, disconnect and try again
 		if (vTimeout < millis())
 		{
 			if (debugger)
@@ -381,11 +436,17 @@ void MQTT_connect() {
 		debugger->print(ap.config.mqttPort);
 		debugger->println();
 	}
+
+	// Wait for the MQTT connection to complete
 	while (!mqtt.connected()) {
+		
+		// Connect to a unsecure or secure MQTT server
 		if ((ap.config.mqttUser == 0 && mqtt.connect(ap.config.mqttClientID)) || 
 			(ap.config.mqttUser != 0 && mqtt.connect(ap.config.mqttClientID, ap.config.mqttUser, ap.config.mqttPass)))
 		{
 			if (debugger) debugger->println("\nSuccessfully connected to MQTT!");
+
+			// Subscribe to the chosen MQTT topic, if set in configuration
 			if (ap.config.mqttSubscribeTopic != 0 && strlen(ap.config.mqttSubscribeTopic) > 0)
 			{
 				mqtt.subscribe(ap.config.mqttSubscribeTopic);
@@ -394,35 +455,47 @@ void MQTT_connect() {
 		}
 		else
 		{
-			if (debugger) debugger->print(".");
-			if (debugger) debugger->print("failed, mqtt.state() = ");
-			if (debugger) debugger->print(mqtt.state());
-			if (debugger) debugger->println(" trying again in 5 seconds");
+			if (debugger)
+			{
+				debugger->print(".");
+				debugger->print("failed, mqtt.state() = ");
+				debugger->print(mqtt.state());
+				debugger->println(" trying again in 5 seconds");
+			}
+
 			// Wait 2 seconds before retrying
 			mqtt.disconnect();
 			delay(2000);
 		}
+
+		// Allow some resources for the WiFi connection
 		yield();
 	}
 }
 
+// Send a simple string embedded in json over MQTT
 void sendMqttData(String data)
 {
+	// Make sure we have configured a publish topic
 	if (ap.config.mqttPublishTopic == 0 || strlen(ap.config.mqttPublishTopic) == 0)
 		return;
 
+	// Make sure we're connected
 	if (!client->connected() || !mqtt.connected()) {
 		MQTT_connect();
 	}
 
-	StaticJsonBuffer<500> jsonBuffer;
+	// Build a json with the message in a "data" attribute
+	DynamicJsonBuffer jsonBuffer;
 	JsonObject& json = jsonBuffer.createObject();
 	json["id"] = WiFi.macAddress();
-	json["up"] = millis() / 1000;
+	json["up"] = millis();
 	json["data"] = data;
 
+	// Stringify the json
 	String msg;
 	json.printTo(msg);
 
+	// Send the json over MQTT
 	mqtt.publish(ap.config.mqttPublishTopic, msg.c_str());
 }
\ No newline at end of file
diff --git a/Code/Arduino/AmsToMqttBridge/accesspoint.cpp b/Code/Arduino/AmsToMqttBridge/accesspoint.cpp
index a1488402..d45e4475 100644
--- a/Code/Arduino/AmsToMqttBridge/accesspoint.cpp
+++ b/Code/Arduino/AmsToMqttBridge/accesspoint.cpp
@@ -71,6 +71,21 @@ void accesspoint::setup(int accessPointButtonPin, Stream& debugger)
 	}
 }
 
+bool accesspoint::loop() {
+	if (isActivated)
+	{
+		//DNS
+		dnsServer.processNextRequest();
+		//HTTP
+		server.handleClient();
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
 /** Handle root or redirect to captive portal */
 void accesspoint::handleRoot() {
 	println("Serving / over http...");
@@ -80,7 +95,7 @@ void accesspoint::handleRoot() {
 	server.sendHeader("Expires", "-1");
 	server.setContentLength(CONTENT_LENGTH_UNKNOWN);
 	server.send(200, "text/html", ""); // Empty content inhibits Content-length header so we have to close the socket ourselves.
-	String html = String("<html>\r\n\r\n<head>\r\n\t<style type=\"text/css\">\r\n\t\tbody div {\r\n\t\t\tfont-family: Roboto, Helvetica Neue Light, Helvetica Neue, Helvetica, Arial, Lucida Grande, sans-serif;\r\n\t\t}\r\n\r\n\t\t.wrapper {\r\n\t\t\twidth: 70%;\r\n\t\t\tposition: absolute;\r\n\t\t\tpadding: 30px;\r\n\t\t\tbackground-color: #FFF;\r\n\t\t\tborder-radius: 1px;\r\n\t\t\tcolor: #333;\r\n\t\t\tborder-color: rgba(0, 0, 0, 0.03);\r\n\t\t\tbox-shadow: 0 2px 2px rgba(0, 0, 0, .24), 0 0 2px rgba(0, 0, 0, .12);\r\n\t\t\tmargin-left: 20px;\r\n\t\t\tmargin-top: 20px;\r\n\t\t}\r\n\r\n\t\tdiv {\r\n\t\t\tpadding-bottom: 5px;\r\n\t\t}\r\n\r\n\t\tinput {\r\n\t\t\tbottom: 30px;\r\n\t\t\tborder: none;\r\n\t\t\tborder-bottom: 1px solid #d4d4d4;\r\n\t\t\tpadding: 10px;\r\n\t\t\twidth: 80%;\r\n\t\t\tbackground: transparent;\r\n\t\t\ttransition: all .25s ease;\r\n\t\t}\r\n\r\n\t\tinput[type=number] {\r\n\t\t\twidth: 70px;\r\n\t\t\tmargin-left: 5px;\r\n\t\t}\r\n\r\n\t\tinput:focus {\r\n\t\t\toutline: none;\r\n\t\t\tborder-bottom: 1px solid #3f51b5;\r\n\t\t}\r\n\r\n\t\th2 {\r\n\t\t\ttext-align: left;\r\n\t\t\tfont-size: 20px;\r\n\t\t\tfont-weight: bold;\r\n\t\t\tletter-spacing: 3px;\r\n\t\t\tline-height: 28px;\r\n\t\t}\r\n\r\n\t\t.submit-button {\r\n\t\t\tposition: absolute;\r\n\t\t\ttext-align: right;\r\n\t\t\tborder-radius: 30px;\r\n\t\t\tborder-bottom-right-radius: 0;\r\n\t\t\tborder-top-right-radius: 0;\r\n\t\t\tbackground-color: #3f51b5;\r\n\t\t\tcolor: #FFF;\r\n\t\t\tpadding: 12px 25px;\r\n\t\t\tdisplay: inline-block;\r\n\t\t\tfont-size: 12px;\r\n\t\t\tfont-weight: bold;\r\n\t\t\tletter-spacing: 2px;\r\n\t\t\tright: 0px;\r\n\t\t\tbottom: 10px;\r\n\t\t\tcursor: pointer;\r\n\t\t\ttransition: all .25s ease;\r\n\t\t\tbox-shadow: 0 2px 2px rgba(0, 0, 0, .24), 0 0 2px rgba(0, 0, 0, .12);\r\n\t\t\twidth: 100px;\r\n\t\t}\r\n\t</style>\r\n</head>\r\n\r\n<body>\r\n\t<form method='post' action='/save'>\r\n\t\t<div class=\"wrapper\">\r\n\t\t\t<div class=\"inner-wrapper\">\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<h2>WiFi</h2>\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='ssid' placeholder=\"SSID\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='password' name='ssidPassword' placeholder=\"Password\">\r\n\t\t\t\t</div>\r\n\t\t\t</div>\r\n\t\t\t<div class=\"inner-wrapper\">\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<h2>AMS Meter</h2>\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='meterType' placeholder=\"Meter Type (1=Kaifa, 2=Kamstrup, 3=Aidon)\">\r\n\t\t\t\t</div>\r\n\t\t\t</div>\r\n\t\t\t<div class=\"inner-wrapper\">\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<h2>MQTT</h2>\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqtt' placeholder=\"Server\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='number' name='mqttPort' placeholder=\"port\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttClientID' placeholder=\"Client ID\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttPublishTopic' placeholder=\"Publish Topic\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttSubscribeTopic' placeholder=\"Subscribe Topic\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttUser' placeholder=\"Username (leave blank for unsecure)\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='password' name='mqttPass' placeholder=\"Password\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input class=\"submit-button\" type='submit' value='save'>\r\n\t\t\t\t</div>\r\n\t\t\t</div>\r\n\t\t</div>\r\n\t</form>\r\n\r\n\t<body>\r\n\r\n</html>");
+	String html = String("<html>\r\n\r\n<head>\r\n\t<style type=\"text/css\">\r\n\t\tbody,\r\n\t\tdiv,\r\n\t\tinput {\r\n\t\t\tfont-family: \"Roboto\", Arial, Lucida Grande;\r\n\t\t}\r\n\r\n\t\t.wrapper {\r\n\t\t\twidth: 50%;\r\n\t\t\tposition: absolute;\r\n\t\t\tpadding: 30px;\r\n\t\t\tbackground-color: #FFF;\r\n\t\t\tborder-radius: 1px;\r\n\t\t\tcolor: #333;\r\n\t\t\tborder-color: rgba(0, 0, 0, 0.03);\r\n\t\t\tbox-shadow: 0 2px 2px rgba(0, 0, 0, .24), 0 0 2px rgba(0, 0, 0, .12);\r\n\t\t\tmargin-left: 20px;\r\n\t\t\tmargin-top: 20px;\r\n\t\t}\r\n\r\n\t\tdiv {\r\n\t\t\tpadding-bottom: 5px;\r\n\t\t}\r\n\r\n\t\tinput {\r\n\t\t\tfont-family: \"Roboto\", \"Helvetica Neue\", sans-serif;\r\n\t\t\tfont-size: 14px;\r\n\t\t\tline-height: 16px;\r\n\t\t\tbottom: 30px;\r\n\t\t\tborder: none;\r\n\t\t\tborder-bottom: 1px solid #d4d4d4;\r\n\t\t\tpadding: 10px;\r\n\t\t\twidth: 80%;\r\n\t\t\tbackground: transparent;\r\n\t\t\ttransition: all .25s ease;\r\n\t\t}\r\n\r\n\t\tinput[type=number] {\r\n\t\t\twidth: 70px;\r\n\t\t\tmargin-left: 5px;\r\n\t\t}\r\n\r\n\t\tinput:focus {\r\n\t\t\toutline: none;\r\n\t\t\tborder-bottom: 1px solid #3f51b5;\r\n\t\t}\r\n\r\n\t\th2 {\r\n\t\t\ttext-align: left;\r\n\t\t\tfont-size: 20px;\r\n\t\t\tfont-weight: bold;\r\n\t\t\tletter-spacing: 3px;\r\n\t\t\tline-height: 28px;\r\n\t\t}\r\n\r\n\t\t.submit-button {\r\n\t\t\tposition: absolute;\r\n\t\t\ttext-align: right;\r\n\t\t\tborder-radius: 20px;\r\n\t\t\tborder-bottom-right-radius: 0;\r\n\t\t\tborder-top-right-radius: 0;\r\n\t\t\tbackground-color: #3f51b5;\r\n\t\t\tcolor: #FFF;\r\n\t\t\tpadding: 12px 25px;\r\n\t\t\tdisplay: inline-block;\r\n\t\t\tfont-size: 12px;\r\n\t\t\tfont-weight: bold;\r\n\t\t\tletter-spacing: 2px;\r\n\t\t\tright: 0px;\r\n\t\t\tbottom: 10px;\r\n\t\t\tcursor: pointer;\r\n\t\t\ttransition: all .25s ease;\r\n\t\t\tbox-shadow: 0 2px 2px rgba(0, 0, 0, .24), 0 0 2px rgba(0, 0, 0, .12);\r\n\t\t\twidth: 100px;\r\n\t\t}\r\n\r\n\t\t.select-style {\r\n\t\t\tborder-top: 10px solid white;\r\n\t\t\tborder-bottom: 1px solid #d4d4d4;\r\n\t\t\tcolor: #ffffff;\r\n\t\t\tcursor: pointer;\r\n\t\t\tdisplay: block;\r\n\t\t\tfont-family: Roboto, \"Helvetica Neue\", sans-serif;\r\n\t\t\tfont-size: 14px;\r\n\t\t\tfont-weight: 400;\r\n\t\t\theight: 16px;\r\n\t\t\tline-height: 14px;\r\n\t\t\tmin-width: 200px;\r\n\t\t\tpadding-bottom: 7px;\r\n\t\t\tpadding-left: 0px;\r\n\t\t\tpadding-right: 0px;\r\n\t\t\tposition: relative;\r\n\t\t\ttext-align: left;\r\n\t\t\twidth: 80%;\r\n\t\t\t-webkit-box-direction: normal;\r\n\t\t\toverflow: hidden;\r\n\t\t\tbackground: #ffffff url(\"data:image/png;base64,R0lGODlhDwAUAIABAAAAAP///yH5BAEAAAEALAAAAAAPABQAAAIXjI+py+0Po5wH2HsXzmw//lHiSJZmUAAAOw==\") no-repeat 98% 50%;\r\n\t\t}\r\n\r\n\t\t.disabled-option {\r\n\t\t\tcolor: #d4d4d4;\r\n\r\n\t\t}\r\n\r\n\t\t.select-style select {\r\n\t\t\tpadding: 5px 8px;\r\n\t\t\twidth: 100%;\r\n\t\t\tborder: none;\r\n\r\n\t\t\tbox-shadow: none;\r\n\t\t\tbackground: transparent;\r\n\t\t\tbackground-image: none;\r\n\t\t\t-webkit-appearance: none;\r\n\t\t}\r\n\r\n\t\t.select-style select:focus {\r\n\t\t\toutline: none;\r\n\t\t\tborder: none;\r\n\t\t}\r\n\r\n\t\t@media only screen and (max-width: 1000px) {\r\n\t\t\t.wrapper {\r\n\t\t\t\twidth: 80%;\r\n\r\n\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\t@media only screen and (max-width: 300px) {\r\n\t\t\t.wrapper {\r\n\t\t\t\twidth: 75%;\r\n\t\t\t}\r\n\t\t}\r\n\r\n\t\t@media only screen and (max-width: 600px) {\r\n\t\t\t.wrapper {\r\n\t\t\t\twidth: 80%;\r\n\t\t\t\tmargin-left: 0px;\r\n\t\t\t\tmargin-top: 0px;\r\n\t\t\t}\r\n\t\t\t.submit-button {\r\n\t\t\t\tbottom: 0px;\r\n\t\t\t\twidth: 70px;\r\n\t\t\t}\r\n\t\t\tinput {\r\n\t\t\t\twidth: 100%;\r\n\t\t\t}\r\n\t\t}\r\n\t</style>\r\n</head>\r\n\r\n<body>\r\n\t<form method='post' action='/save'>\r\n\r\n\t\t<div class=\"wrapper\">\r\n\r\n\t\t\t<div class=\"inner-wrapper\">\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<h2>WiFi</h2>\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='ssid' placeholder=\"SSID\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='password' name='ssidPassword' placeholder=\"Password\">\r\n\t\t\t\t</div>\r\n\t\t\t</div>\r\n\t\t\t<div class=\"inner-wrapper\">\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<h2>Meter Type</h2>\r\n\t\t\t\t</div>\r\n\t\t\t\t<div class=\"select-style\">\r\n\t\t\t\t\t<select name=\"meterType\">\r\n\t\t\t\t\t\t<option value=\"0\" disabled=\"true\" selected class=\"disabled-option\"> SELECT TYPE </option>\r\n\t\t\t\t\t\t<option value=\"1\">Kaifa</option>\r\n\t\t\t\t\t\t<option value=\"2\">Aidon</option>\r\n\t\t\t\t\t\t<option value=\"3\">Kamstrup</option>\r\n\t\t\t\t\t</select>\r\n\t\t\t\t</div>\r\n\t\t\t</div>\r\n\t\t\t<div class=\"inner-wrapper\">\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<h2>MQTT</h2>\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqtt' placeholder=\"Server\">\r\n\t\t\t\t\t<input type='number' name='mqttPort' placeholder=\"port\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttClientID' placeholder=\"Client ID\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttPublishTopic' placeholder=\"Publish Topic\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttSubscribeTopic' placeholder=\"Subscribe Topic\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='text' name='mqttUser' placeholder=\"Username (leave blank for unsecure)\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input type='password' name='mqttPass' placeholder=\"Password\">\r\n\t\t\t\t</div>\r\n\t\t\t\t<div>\r\n\t\t\t\t\t<input class=\"submit-button\" type='submit' value='save'>\r\n\t\t\t\t</div>\r\n\t\t\t</div>\r\n\t\t</div>\r\n\t</form>\r\n\r\n\t<body>\r\n\r\n</html>");
 	server.sendContent(html);
 	server.client().stop(); // Stop is needed because we sent no content length
 }
@@ -145,22 +160,6 @@ void accesspoint::handleSave() {
 	}
 }
 
-bool accesspoint::loop() {
-	if (isActivated)
-	{
-		//DNS
-		dnsServer.processNextRequest();
-		//HTTP
-		server.handleClient();
-		return true;
-	}
-	else
-	{
-		return false;
-	}
-}
-
-
 size_t accesspoint::print(const char* text)
 {
 	if (debugger) debugger->print(text);