  */
 void loop() {
 	check();
-	{
-		int connectCounter = 0;
-		while (!mqtt->isConnected()) {
-			connect();
-			if (!mqtt->isConnected()) {
-				connectCounter++;
-				if (connectCounter < MQTT_CONNECT_RETRIES_QTY ) {
-					LOG_PRINTFLN("Reconnect in %u ms", MQTT_CONNECT_RETRIES_IDLE_PERIOD_MS);
-					Lpm::idle(MQTT_CONNECT_RETRIES_IDLE_PERIOD_MS);
-				} else {
-					LOG_PRINTFLN("ERROR, Max retries qty has been reached => reset in %lu ms",
-							MQTT_DISCONNECTED_IDLE_PERIOD_MS);
-					networkHibernate(NETWORK_HIBERNATE_DELAY_MS);
-					Lpm::idle(MQTT_DISCONNECTED_IDLE_PERIOD_MS - NETWORK_HIBERNATE_DELAY_MS);
-					reset();
-				}
+	if (!mqtt->isConnected()) {
+		// Not connected
+		// Clean buffers and give some time for network
+		mqtt->yield();
+		// Connect
+		connect();
+		if (!mqtt->isConnected()) {
+			if (++connectCounter > MQTT_CONNECT_RETRIES_QTY ) {
+				LOG_PRINTFLN("ERROR, Max retries qty has been reached => reset");
+				reset();
+				// Execution must stop at this point
 			}
-		}
-	}
-	{
-		const uint8_t bufSize = MQTT_MAX_PAYLOAD_SIZE;
-		char buf[bufSize];
-		memset(buf, 0, bufSize);
-		// build message payload
-		{
-			const int NUMBER_OF_SENSORS = 2;
-			// See JSON_OBJECT_SIZE and JSON_ARRAY_SIZE to predict buffer size
-			// lets make double the prediction
-			const int BUFFER_SIZE = 2 * (JSON_OBJECT_SIZE(3)
-					+ JSON_ARRAY_SIZE(NUMBER_OF_SENSORS)
-					+ NUMBER_OF_SENSORS*JSON_OBJECT_SIZE(1));
-			StaticJsonBuffer<BUFFER_SIZE> jsonBuffer;
-			JsonArray& data = jsonBuffer.createArray();
-			JsonObject& temp = jsonBuffer.createObject();
-			{
-				SensorValue v = sensorTemperature->getData();
-				if (sensorTemperature->verify(v)) {
-					temp["temp"] = v;
-					data.add(temp);
-				}
+			networkHibernate(NETWORK_HIBERNATE_DELAY_MS);
+			LOG_PRINTFLN("Reconnect in %lu ms", MQTT_DISCONNECTED_IDLE_PERIOD_MS);
+			Lpm::idle(MQTT_DISCONNECTED_IDLE_PERIOD_MS);
+			networkWakeUp(NETWORK_WAKE_UP_DELAY_MS);
+		} else {
+			// Do the publish immediately after reconnect
+			firstPublish = true;
+			// Listen for configuration
+			LOG_PRINTFLN("Waiting the configuration for %lu ms", MQTT_LISTEN_TIME_MS);
+			mqtt->yield(MQTT_LISTEN_TIME_MS);
+		}
+	} else {
+		// Connected
+		// Check if time to Publish
+		if (firstPublish || isTimePassed(publishTs, publishPeriodMs)) {
+			const int bufferSize = MQTT_MAX_PAYLOAD_SIZE;
+			char buffer[bufferSize];
+			memset(buffer, 0, bufferSize);
+			// Build message payload
+			buildMessagePayload(buffer, bufferSize);
+			// Build message
+			MqttClient::Message message;
+			message.qos = MQTT_PUBLISH_QOS;
+			message.retained = false;
+			message.dup = false;
+			message.payload = (void*) buffer;
+			message.payloadLen = strlen(buffer) + 1;
+			// Publish
+			MqttClient::Error::type rc = mqtt->publish(MQTT_PUBLISH_TOPIC, message);
+			if (rc != MqttClient::Error::SUCCESS) {
+				LOG_PRINTFLN("ERROR, Publish, rc:%i", rc);
+			} else {
+				publishTs = time.millis();
+				firstPublish = false;
 			}
-			JsonObject& humid = jsonBuffer.createObject();
-			{
-				SensorValue v = sensorHumidity->getData();
-				if (sensorHumidity->verify(v)) {
-					humid["humid"] = v;
-					data.add(humid);
-				}
+		}
+		// Idle until next event
+		LOG_PRINTFLN("Idle for %lu ms", min(timeLeft(publishTs, publishPeriodMs), mqtt->getIdleInterval()));
+		networkHibernate(NETWORK_HIBERNATE_DELAY_MS);
+		Lpm::idle(min(timeLeft(publishTs, publishPeriodMs), mqtt->getIdleInterval()));
+		networkWakeUp(NETWORK_WAKE_UP_DELAY_MS);
+		// Determine the next event
+		if (timeLeft(publishTs, publishPeriodMs) > mqtt->getIdleInterval()) {
+			// Need to call yield
+			while(mqtt->isConnected() && mqtt->getIdleInterval() < MQTT_COMMAND_TIMEOUT_MS) {
+				mqtt->yield();
 			}
-			JsonObject& root = jsonBuffer.createObject();
-			root["v"] = 1;
-			root["id"] = SENSOR_ID;
-			root["data"] = data;
-			// write to buffer
-			size_t size = root.printTo(buf, bufSize);
-			if (size+1 >= bufSize) {
-				LOG_PRINTFLN("WARN, Big payload: %d", size);
-			}
-		}
-		// build message
-		MQTT::Message message;
-		message.qos = MQTT_PUBLISH_QOS;
-		message.retained = false;
-		message.dup = false;
-		message.payload = (void*) buf;
-		message.payloadlen = strlen(buf) + 1;
-		int rc = mqtt->publish(MQTT_PUBLISH_TOPIC, message);
-		if (rc != 0) {
-			LOG_PRINTFLN("ERROR, MQTT publish, rc:%d", rc);
-		}
-	}
-	LOG_PRINTFLN("Idle for %lu ms", publishPeriodMs);
-	mqtt->yield(MQTT_LISTEN_TIME_MS);
-	networkHibernate(NETWORK_HIBERNATE_DELAY_MS);
-	Lpm::idle(publishPeriodMs
-			- MQTT_LISTEN_TIME_MS
-			- NETWORK_HIBERNATE_DELAY_MS
-			- NETWORK_WAKE_UP_DELAY_MS
-	);
-	networkWakeUp(NETWORK_WAKE_UP_DELAY_MS);
+		}
+	}
+}
+
+void buildMessagePayload(char* buffer, int size) {
+	const int NUMBER_OF_ROOT_PARAMETERS = 3;
+	const int NUMBER_OF_SENSORS = 2;
+	const int NUMBER_OF_SENSORS_PARAMETERS = 1;
+	// See JSON_OBJECT_SIZE and JSON_ARRAY_SIZE to predict buffer size
+	// Let's make double the prediction
+	const int BUFFER_SIZE = 2 * (JSON_OBJECT_SIZE(NUMBER_OF_ROOT_PARAMETERS)
+			+ JSON_ARRAY_SIZE(NUMBER_OF_SENSORS)
+			+ NUMBER_OF_SENSORS*JSON_OBJECT_SIZE(NUMBER_OF_SENSORS_PARAMETERS));
+	StaticJsonBuffer<BUFFER_SIZE> jsonBuffer;
+	JsonArray& data = jsonBuffer.createArray();
+	JsonObject& temp = jsonBuffer.createObject();
+	{
+		SensorValue v = sensorTemperature->getData();
+		if (sensorTemperature->verify(v)) {
+			temp["temp"] = v;
+			data.add(temp);
+		}
+	}
+	JsonObject& humid = jsonBuffer.createObject();
+	{
+		SensorValue v = sensorHumidity->getData();
+		if (sensorHumidity->verify(v)) {
+			humid["humid"] = v;
+			data.add(humid);
+		}
+	}
+	JsonObject& root = jsonBuffer.createObject();
+	root["v"] = 1;
+	root["id"] = SENSOR_ID;
+	root["data"] = data;
+	// Write to buffer
+	root.printTo(buffer, size);
 }
 
 void check() {
-	unsigned long time = millis();
 	LOG_PRINTFLN("#################################");
 	LOG_PRINTFLN("###      Periodic check       ###");
 	LOG_PRINTFLN("### Memory Free :    %.5u B  ###", freeMemory());
-	LOG_PRINTFLN("### Time        : %.8lu Ms ###", time);
+	LOG_PRINTFLN("### Time        : %.8lu Ms ###", time.millis());
 	LOG_PRINTFLN("### Period      : %.8lu Ms ###", publishPeriodMs);
 	LOG_PRINTFLN("#################################");
 }

#define MQTT_MAX_PACKET_SIZE						128
#define MQTT_MAX_PAYLOAD_SIZE						96
#define MQTT_MAX_MESSAGE_HANDLERS					1
#define MQTT_COMMAND_TIMEOUT_MS						8000				// 8 sec
#define MQTT_KEEP_ALIVE_INTERVAL_SEC				5*60				// 5 min
#define MQTT_CLIENT_ID								SENSOR_ID
#define MQTT_SUBSCRIBE_QOS							MQTT::QOS1
#define MQTT_SUBSCRIBE_TOPIC_CFG					"butler/sensor/"SENSOR_ID"/config"
#define MQTT_PUBLISH_QOS							MQTT::QOS0
#define MQTT_PUBLISH_TOPIC							"butler/sensor/"SENSOR_ID"/data"
#define MQTT_LISTEN_TIME_MS							1000				// 1 sec
#define MQTT_PUBLISH_PERIOD_MS						3*60*1000L			// 3 min
#define MQTT_PUBLISH_PERIOD_MAX_MS					((MQTT_KEEP_ALIVE_INTERVAL_SEC - 5) * 1000L)
#define MQTT_CONNECT_RETRIES_QTY					5
#define MQTT_CONNECT_RETRIES_IDLE_PERIOD_MS			5000				// 5 sec
#define MQTT_DISCONNECTED_IDLE_PERIOD_MS			((MQTT_KEEP_ALIVE_INTERVAL_SEC + 5) * 1000L)
#define NETWORK_UART_IDLE_PERIOD_LONG_MS			16
#define NETWORK_UART_IDLE_PERIOD_SHORT_MS			2
#define NETWORK_HIBERNATE_DELAY_MS					2000
#define NETWORK_WAKE_UP_DELAY_MS					10000
#define LPM_MODE									LPM_MODE_IDLE
#define HW_UART_SPEED								57600
#define SW_UART_SPEED								57600


////////// OBJECTS //////////
Network* network = NULL;
MQTT::Client<Network, Countdown, MQTT_MAX_PACKET_SIZE, MQTT_MAX_MESSAGE_HANDLERS>* mqtt =
		NULL;
DHT* dht = NULL;
Sensor* sensorTemperature = NULL;
Sensor* sensorHumidity = NULL;
unsigned long publishPeriodMs = MQTT_PUBLISH_PERIOD_MS;

SwUartConfig swUartConfig = {SW_UART_SPEED, PIN_SW_UART_RX, PIN_SW_UART_TX};
SwUart* swUart = NULL;

HwUartConfig hwUartConfig = {HW_UART_SPEED};
HwUart* hwUart = NULL;

//// DECLARATION ////
void(* reset) (void) = 0;//declare reset function at address 0
void check();
void connect();
void processMessageCfg(MQTT::MessageData& md);
void networkHibernate(uint32_t);
void networkWakeUp(uint32_t);

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

#if LOG_ENABLED
	/// HW UART ///
	{
		hwUart = new HwUart(hwUartConfig);
	}
#endif

	/// SF UART ///
	{
		swUart = new SwUart(swUartConfig);
	}

#if LOG_ENABLED
	//// LOG ////
	{
		Logger::init(*hwUart);
	}
#endif

	//// LPM ////
	{

 * Purpose: Arduino "Main" functions
 *
 *******************************************************************************
 * Copyright Oleg Kovalenko 2014, 2015, 2017.
 *
 * Distributed under the MIT License.
 * (See accompanying file LICENSE or copy at http://opensource.org/licenses/MIT)
