 SoftwareSerial swSer(RX, TX, false, 255);
 
 // Network settings
-const char* ssid              = "VM146928-2G";
-const char* password          = "gbwnhajc";
+const char *ssid = "some-SSID";
+const char *password = "some-PSK";
 
 // MQTT connect settings
-const char* mqtt_server       = "192.168.1.3";
-const char* mqtt_username     = "device"; 
-const char* mqtt_password     = "didmsiskf0wdo";
-
-//// Network settings
-//const char* ssid              = "some-SSID";
-//const char* password          = "some-PSK";
-//
-//// MQTT connect settings
-//const char* mqtt_server       = "some-MQTT-server";
-//const char* mqtt_username     = "some-MQTT-username"; 
-//const char* mqtt_password     = "some-MQTT-password";
-const char* mqtt_client_id    = "Mesquitto Gateway"; //String("Mesquitto Gateway "+ ESP.getChipId()).c_str();
-//const char* mqtt_will_topic   = ("/1/gateways/"+getMac()+"/disconnected").c_str();
-const char* mqtt_will_payload = "1";
-const int   mqtt_will_qos     = 1;
-const bool  mqtt_will_retain  = true; 
+const char *mqtt_server = "some-MQTT-server";
+const char *mqtt_username = "some-MQTT-username";
+const char *mqtt_password = "some-MQTT-password";
+const char *mqtt_client_id = "Mesquitto Gateway";
+const char *mqtt_will_topic = "/meshquitto-gateway/disconnected";
+const char *mqtt_will_payload = "1";
+const int mqtt_will_qos = 1;
+const bool mqtt_will_retain = true;
 
 // WiFi and MQTT client instantiation
 WiFiClientSecure espClient;
 PubSubClient client(espClient);
 
 // Buffers to store messages
-CustomList<String> mqttMessageBuffer;         // Stores list of all messages queued to be sent to MQTT broker
-CustomList<String> meshMessageBuffer;         // Stores list of all messages queued to be forwarded to Mesh gateway
+CustomList<String> mqttMessageBuffer; // Stores list of all messages queued to be sent to MQTT broker
+CustomList<String> meshMessageBuffer; // Stores list of all messages queued to be forwarded to Mesh gateway
 
 // Timestamp to store last time a message was sent to Mesh
 long lastMsg = 0;
 
 // Global flags used for control
-volatile bool _sending                 = false;                // Flag set when sending to Mesh
-volatile bool _receiving               = false;              // Flag set when receiving from mesh
-bool _empty_mqtt_buffer_irq   = false;  // Flag set when 
+volatile bool _sending = false;      // Flag set when sending to Mesh
+volatile bool _receiving = false;    // Flag set when receiving from mesh
+bool _empty_mqtt_buffer_irq = false; // Flag set when
 
 //String TX_FSM_STATE = TX_FSM_READY;
 //Ticker RX_Interrupt_Ticker;

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


/************************************************************************/
/* 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;

  bool topicParsed = false;
  int parserPos = 0;
  int i = 0;
  while (!topicParsed)
  {
    int startPos = parserPos + 1;
    //Serial.print("indexOf: "); Serial.println(topic_str.indexOf("/", startPos));
    int endPos = topic_str.indexOf("/", startPos);
    if (endPos == -1)
    {
      endPos = topic_str.length();
      topicParsed = true;
    }
