 void connect() {
 	int rc = network->connect(MQTT_HOST, MQTT_PORT);
 	if (rc != 0) {
-		LOG_PRINTFLN("ERROR, TCP connect, rc:%d", rc);
+		LOG_PRINTFLN("ERROR, TCP connect, rc:%i", rc);
+	} else {
+		LOG_PRINTFLN("Connect");
+		MqttClient::Error::type rc = MqttClient::Error::SUCCESS;
+		MqttClient::ConnectResult connectResult;
+		{
+			MQTTPacket_connectData options = MQTTPacket_connectData_initializer;
+			options.MQTTVersion = 4;
+			options.clientID.cstring = (char*)MQTT_CLIENT_ID;
+			options.cleansession = true;
+			options.keepAliveInterval = MQTT_KEEP_ALIVE_INTERVAL_SEC;
+			rc = mqtt->connect(options, connectResult);
+		}
+		if (rc != MqttClient::Error::SUCCESS) {
+			LOG_PRINTFLN("ERROR, Connect, rc:%i", rc);
+		} else {
+			const char* topic = MQTT_SUBSCRIBE_TOPIC_CFG;
+			LOG_PRINTFLN("Subscribe to %s", topic);
+			rc = mqtt->subscribe(topic, MQTT_SUBSCRIBE_QOS, processMessageConfig);
+			if (rc != MqttClient::Error::SUCCESS) {
+				LOG_PRINTFLN("ERROR, Subscribe, rc:%i", rc);
+				LOG_PRINTFLN("Drop connection");
+				mqtt->disconnect();
+			} else {
+				// Success
+				return;
+			}
+		}
+	}
+	// Failure => disconnect
+	network->disconnect();
+}
+
+void processMessageConfig(MqttClient::MessageData& md) {
+	const MqttClient::Message& msg = md.message;
+	char payload[msg.payloadLen + 1];
+	memcpy(payload, msg.payload, msg.payloadLen);
+	payload[msg.payloadLen] = '\0';
+	LOG_PRINTFLN(
+		"Configuration arrived: retained %u, payload:[%s]",
+		msg.retained, payload
+	);
+	const int NUMBER_OF_ROOT_PARAMETERS = 1;
+	// See JSON_OBJECT_SIZE to predict buffer size
+	// Let's make double the prediction
+	const int BUFFER_SIZE = 2 * (JSON_OBJECT_SIZE(NUMBER_OF_ROOT_PARAMETERS));
+	StaticJsonBuffer<BUFFER_SIZE> jsonBuffer;
+	JsonObject& root = jsonBuffer.parseObject((char*)payload);
+	// Check if parsing succeeds
+	if (!root.success()) {
+		LOG_PRINTFLN("ERROR, Can't pars configuration");
 		return;
 	}
-	LOG_PRINTFLN("MQTT connect");
-	MQTTPacket_connectData options = MQTTPacket_connectData_initializer;
-	options.MQTTVersion = 3;
-	options.clientID.cstring = (char*)MQTT_CLIENT_ID;
-	options.cleansession = true;
-	options.keepAliveInterval = MQTT_KEEP_ALIVE_INTERVAL_SEC;
-	rc = mqtt->connect(options);
-	if (rc != 0) {
-		LOG_PRINTFLN("ERROR, MQTT connect, rc:%d", rc);
-		return;
-	}
-	LOG_PRINTFLN("MQTT subscribe to %s", MQTT_SUBSCRIBE_TOPIC_CFG);
-	rc = mqtt->subscribe(MQTT_SUBSCRIBE_TOPIC_CFG, MQTT_SUBSCRIBE_QOS, processMessageCfg);
-	if (rc != 0) {
-		LOG_PRINTFLN("ERROR, MQTT subscribe, rc:%d", rc);
-		LOG_PRINTFLN("MQTT drop connection");
-		mqtt->disconnect();
-	}
-}
-
-void printMessage(const char* topic, const char* payload, MQTT::Message msg) {
-	LOG_PRINTFLN(
-			"Message arrived: qos %d, retained %d, dup %d, packetid %d, topic:[%s], payload:[%s]",
-			msg.qos, msg.retained, msg.dup, msg.id, topic, payload);
-}
-
-void processMessageCfg(MQTT::MessageData& md) {
-	const MQTT::Message& msg = md.message;
-	const char* tokenSep = ",";
-	const char blockSep = ':';
-	// TODO: change format to JSON
-	// format is "name1:value,name2:value,nameN:value"
-	char payload[msg.payloadlen + 1];
-	memcpy(payload, msg.payload, msg.payloadlen);
-	payload[msg.payloadlen] = '\0';
-	printMessage(MQTT_SUBSCRIBE_TOPIC_CFG, payload, msg);
-	char* token = strtok(payload, tokenSep);
-	while (token != 0) {
-		String name;
-		String value;
-		{
-			String block(token);
-			int sep = block.indexOf(blockSep);
-			name = block.substring(0, sep);
-			value = block.substring(sep + 1);
-		}
-		if (name.equals("period")) {
-			char buf[value.length() + 1];
-			value.toCharArray(buf, value.length() + 1);
-			unsigned long tmp = value.toInt();
-			if (tmp > 0) {
-				publishPeriodMs = (tmp > MQTT_PUBLISH_PERIOD_MAX_MS) ?
-				MQTT_PUBLISH_PERIOD_MAX_MS : tmp;
-			}
-		}
-		// Find the next token
-		token = strtok(0, tokenSep);
-	}
-}
-
-void networkHibernate(uint32_t delay) {
+	publishPeriodMs = root["period"];
+}
+
+void networkHibernate(unsigned long delay) {
 	Lpm::idle(delay);
 	digitalWrite(PIN_LPM_NETWORK, HIGH);
 }
 
-void networkWakeUp(uint32_t delay) {
+void networkWakeUp(unsigned long delay) {
 	digitalWrite(PIN_LPM_NETWORK, LOW);
 	Lpm::idle(delay);
 }
+

/* External Includes */
#include <MemoryFree.h>
#include <Arduino.h>
#include <Countdown.h>
#include <MQTTClient.h>
#include <SoftwareSerial.h>
#include <ArduinoJson.h>
#include <DHT.h>

#define MQTT_MAX_PACKET_SIZE						128
#define MQTT_MAX_PAYLOAD_SIZE						96
#define MQTT_MAX_MESSAGE_HANDLERS					1
#define MQTT_COMMAND_TIMEOUT_MS						(8*1000L)
#define MQTT_KEEP_ALIVE_INTERVAL_SEC				(MQTT_PUBLISH_PERIOD_MS/1000L*3)
#define MQTT_CLIENT_ID								SENSOR_ID
#define MQTT_SUBSCRIBE_QOS							MqttClient::QOS1
#define MQTT_SUBSCRIBE_TOPIC_CFG					"test/sensor/" SENSOR_ID "/config"
#define MQTT_PUBLISH_QOS							MqttClient::QOS0
#define MQTT_PUBLISH_TOPIC							"test/sensor/" SENSOR_ID "/data"
#define MQTT_LISTEN_TIME_MS							(5*1000L)
#define MQTT_PUBLISH_PERIOD_MS						(1*60*1000L)
#define MQTT_CONNECT_RETRIES_QTY					5
#define MQTT_DISCONNECTED_IDLE_PERIOD_MS			(2*60*1000L)
#define NETWORK_HIBERNATE_DELAY_MS					100L
#define NETWORK_WAKE_UP_DELAY_MS					100L
#define LPM_MODE									LPM_MODE_IDLE
#define HW_UART_SPEED								57600L
#define SW_UART_SPEED								9600L

////////// OBJECTS //////////
class Time: public MqttClient::Time {
public:
	unsigned long millis() const {
		return ::millis();
	}
} time;

Network *network									= NULL;
MqttClient *mqtt									= NULL;
Sensor* sensorTemperature							= NULL;
Sensor* sensorHumidity								= NULL;
unsigned long publishPeriodMs						= MQTT_PUBLISH_PERIOD_MS;
unsigned long publishTs								= 0;
int connectCounter									= 0;
bool firstPublish									= false;

//// DECLARATION ////
void(* reset) (void) = 0;//declare reset function at address 0
void check();
void connect();
void buildMessagePayload(char* buffer, int size);
void processMessageConfig(MqttClient::MessageData& md);
void networkHibernate(unsigned long);
void networkWakeUp(unsigned long);

////////// IMPLEMENTATION //////////
unsigned long timeElapsed(unsigned long startTs) {
	return time.millis() - startTs;
}

bool isTimePassed(unsigned long startTs, unsigned long duration) {
	return timeElapsed(startTs) >= duration;
}

unsigned long timeLeft(unsigned long startTs, unsigned long duration) {
	return isTimePassed(startTs, duration) ? 0 : duration - timeElapsed(startTs);
}

/** Called once at startup */
void setup() {
	////// INIT //////

	/// HW UART ///
	Uart *hwUart = new HwUart({HW_UART_SPEED});

	/// SF UART ///
	Uart *swUart = new SwUart({SW_UART_SPEED, PIN_SW_UART_RX, PIN_SW_UART_TX});

	//// LOG ////
	{
		Logger::init(*hwUart);
	}

	//// LPM ////
	{
