 #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_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							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_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_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 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								57600
-#define SW_UART_SPEED								57600
-
+#define HW_UART_SPEED								57600L
+#define SW_UART_SPEED								9600L
 
 ////////// 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;
+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 processMessageCfg(MQTT::MessageData& md);
-void networkHibernate(uint32_t);
-void networkWakeUp(uint32_t);
+void buildMessagePayload(char* buffer, int size);
+void processMessageConfig(MqttClient::MessageData& md);
+void networkHibernate(unsigned long);
+void networkWakeUp(unsigned long);
 
 ////////// IMPLEMENTATION //////////
-/**
- * Called once at startup
- */
+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 //////
 
-#if LOG_ENABLED
 	/// HW UART ///
-	{
-		hwUart = new HwUart(hwUartConfig);
-	}
-#endif
+	Uart *hwUart = new HwUart({HW_UART_SPEED});
 
 	/// SF UART ///
-	{
-		swUart = new SwUart(swUartConfig);
-	}
-
-#if LOG_ENABLED
+	Uart *swUart = new SwUart({SW_UART_SPEED, PIN_SW_UART_RX, PIN_SW_UART_TX});
+
 	//// LOG ////
 	{
 		Logger::init(*hwUart);
 	}
-#endif
 
 	//// LPM ////
 	{

void connect() {
	int rc = network->connect(MQTT_HOST, MQTT_PORT);
	if (rc != 0) {
		LOG_PRINTFLN("ERROR, TCP connect, rc:%d", rc);
		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) {
	Lpm::idle(delay);
	digitalWrite(PIN_LPM_NETWORK, HIGH);
}

void networkWakeUp(uint32_t delay) {
	digitalWrite(PIN_LPM_NETWORK, LOW);
	Lpm::idle(delay);
}

 * 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)
