/*********************MQTT connect **************************************/
bool mqtt_connect() {
  const int8_t maxRetryMqtt = 5;
  int8_t currRetryMqtt = 0;  
  char mqttBrokerBuf[16]; //ToDo Legacy IPv4

  mqttBroker.toString().toCharArray( mqttBrokerBuf, mqttBroker.toString().length()+1 );
#ifdef SERIAL_DEBUG
  //Serial.println("MQTT Status: " + String(mqttClient.state()));
#endif

  if (mqttClient.connected()) {
    mqttCheck = true;
    return true;
  }

#ifdef SERIAL_DEBUG
  Serial.println("Connecting to MQTT Broker...\t"+ String(mqttBrokerBuf));
#endif

#ifdef OLED_OUTPUT
  display.clear();
  //display.drawStringMaxWidth(0, 0, 128, "MQTT: conn");
#endif
  //set options, may be obsolete
  //mqttClient.setOptions(2, true, 750);
  mqttClient.begin(mqttBrokerBuf, MQTT_PORT, mqttSocket);
  mqttClient.onMessage(messageReceived);
  mqttClient.setWill(MQTT_WILL_TOPIC, MQTT_WILL_MESSAGE, MQTT_WILL_RETAIN, MQTT_WILL_QOS);
  bool retVal = mqttClient.connect(MQTT_CLIENTID, MQTT_USERNAME, MQTT_PASSWORD);
  delay(10);
  while (currRetryMqtt <= maxRetryMqtt && !mqttClient.connect(MQTT_CLIENTID, MQTT_USERNAME, MQTT_PASSWORD)) 
  { // connect will return 0 for connected
    currRetryMqtt++;
    if (mqttClient.connected()) {
      currRetryMqtt=0;
      return true;
    }
    else {
      #ifdef SERIAL_DEBUG
      if (currRetryMqtt == 1) {
        Serial.print("Retrying MQTT connection in 1 seconds \t.");
      }
      else {
        Serial.print(".");
      }
      #endif
      delay(1000);// wait 1 seconds
    }
  }
  retVal = mqttClient.connected();

#ifdef OLED_OUTPUT
    display.drawStringMaxWidth(0, 32, 128, "MQTT: Error " + String(retVal));
    display.display();
#endif

#ifdef OLED_OUTPUT
    display.clear();
#endif
    return retVal;
}

bool mqtt_disconnect() {
  bool retVal = mqttClient.disconnect();
  delay(10);
  mqttClient.loop();
  delay(10);
#ifdef SERIAL_DEBUG
  if ( retVal ) {
    Serial.println("mqtt disconnected");
  }
  else {
    Serial.println("mqtt NOT disconnected");
  }
#endif
  return retVal;
}