 
   // Format and push message to buffer
   String mesh_topic;
-  for(int i=3; i<subtopicNo; i++){
-    mesh_topic+="/";
-    mesh_topic+=subtopics[i];
+  for (int i = 3; i < subtopicNo; i++)
+  {
+    mesh_topic += "/";
+    mesh_topic += subtopics[i];
   }
   meshMessageBuffer.push_back(jsonMqttMessage(mesh_topic, value));
 }
 /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
-
 
 /*************************************************************************/
 /* MQTT reconnect funtion: Get called when a connection is lost or first */
 /* started.                                                              */
 /*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
-void reconnect() {
+void reconnect()
+{
   // Loop until we're reconnected
-  while (!client.connected()) {
+  while (!client.connected())
+  {
     Serial.print("Attempting MQTT connection...");
     // Attempt to connect
-    if (client.connect(mqtt_client_id, mqtt_username, mqtt_password, ("/1/gateways/"+getMac()+"/disconnected").c_str(), mqtt_will_qos, mqtt_will_retain, mqtt_will_payload)) {
+    if (client.connect(mqtt_client_id, mqtt_username, mqtt_password, mqtt_will_topic, mqtt_will_qos, mqtt_will_retain, mqtt_will_payload))
+    {
       Serial.println("connected");
       // Once connected, publish an announcement...
-      client.publish(String("/1/gateways/"+getMac()+"/disconnected").c_str(), String("0").c_str());
+      client.publish(String("/1/gateways/" + getMac() + "/disconnected").c_str(), String("0").c_str());
       // ... and resubscribe
-      client.subscribe(String("/1/gateways/"+getMac()+"/+/sensors/+/input/value").c_str());
-    } else {
+      client.subscribe(String("/1/gateways/" + getMac() + "/+/sensors/+/input/value").c_str());
+    }
+    else
+    {
       Serial.print("failed, rc=");
       Serial.print(client.state());
       Serial.println(" try again in 5 seconds");

}
/************************************************************************/


/************************************************************************/
/* Prints available heap memory to Serial                               */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void printHeap(){                                                       
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
  Serial.print("Free Heap: "); Serial.println(ESP.getFreeHeap());
}
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/


/************************************************************************/
/* Prints the number of queued messages to Serial                       */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void printQueueSizes(){
  Serial.print("MQTT queue: "); Serial.println(mqttMessageBuffer.size());
  Serial.print("Mesh queue: "); Serial.println(meshMessageBuffer.size());
}
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/


/************************************************************************/
/* Computes CRC16 of given data, then appends it to data and returns    */
/* the resulting string                                                 */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
String getCRCString(String data){
  Crc16 crc;
  byte * data_buf = (unsigned char*)data.c_str();
  crc.clearCrc();
  unsigned short value = crc.XModemCrc(data_buf,0,data.length());
  String crc_value = String(value, HEX);
  while(crc_value.length()<4){
    crc_value = "0" + crc_value;
  }
  data += crc_value;
  return data;
}
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/


/************************************************************************/
/* Reads a string of data with it's CRC16 appended to the end, then     */
/* computes a new CRC16 based on the raw data and returns true if       */
/* the two CRC16 values match.                                          */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
bool checkCRC(String dataPlusCRC ){
  Crc16 crc;
  crc.clearCrc();
  byte data[dataPlusCRC.length()-4];
  for(int i=0; i<dataPlusCRC.length()-4; i++){
    data[i] = (byte) dataPlusCRC[i];
  }
  unsigned short crc_1 = strtoul(dataPlusCRC.substring(dataPlusCRC.length()-4).c_str(), NULL, 16);
  unsigned short crc_check = crc.XModemCrc(data,0,dataPlusCRC.length()-4);
  if(crc_1==crc_check){
    return true; 
  }
  return false;
}
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/


/*************************************************************************/
/* Reads topic and payload of MQTT message and formats it as JSON string */
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
String jsonMqttMessage(String topic, String payload){
  StaticJsonBuffer<500> jsonBuffer;
  JsonObject& rootFS2 = jsonBuffer.createObject();
  rootFS2["topic"] = topic;
  rootFS2["payload"] = payload;
  String json_msg;


  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
