OpenMQTTGateway/main/gatewayBT.cpp

/*
  OpenMQTTGateway  - ESP8266 or Arduino program for home automation

   Act as a gateway between your 433mhz, infrared IR, BLE, LoRa signal and one interface like an MQTT broker
   Send and receiving command by MQTT

  This gateway enables to:
 - publish MQTT data to a topic related to BLE devices data

    Copyright: (c)Florian ROBERT

    This file is part of OpenMQTTGateway.

    OpenMQTTGateway is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    OpenMQTTGateway is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

Thanks to wolass https://github.com/wolass for suggesting me HM 10 and dinosd https://github.com/dinosd/BLE_PROXIMITY for inspiring me how to implement the gateway
*/
#include "User_config.h"

#ifdef ZgatewayBT
#  include "TheengsCommon.h"

SemaphoreHandle_t semaphoreCreateOrUpdateDevice;
SemaphoreHandle_t semaphoreBLEOperation;
QueueHandle_t BLEQueue;
unsigned long scanCount = 0;
#  include <NimBLEAdvertisedDevice.h>
#  include <NimBLEDevice.h>
#  include <NimBLEScan.h>
#  include <NimBLEUtils.h>
#  include <esp_bt.h>
#  include <esp_wifi.h>

#  include <atomic>

#  include "TheengsCommon.h"
#  include "config_mqttDiscovery.h"
#  include "gatewayBLEConnect.h"
#  include "soc/timer_group_reg.h"
#  include "soc/timer_group_struct.h"

using namespace std;

// Global struct to store live BT configuration data
BTConfig_s BTConfig;

#  if BLEDecoder
#    include <decoder.h>
#    if BLEDecryptor
#      include "mbedtls/aes.h"
#      include "mbedtls/ccm.h"
#    endif
TheengsDecoder decoder;
#  endif

static TaskHandle_t xCoreTaskHandle;
static TaskHandle_t xProcBLETaskHandle;

struct decompose {
  int start;
  int len;
  bool reverse;
};

vector<BLEAction> BLEactions;

vector<BLEdevice*> devices;
int newDevices = 0;

static BLEdevice NO_BT_DEVICE_FOUND = {
    {0},
    0,
    false,
    false,
    false,
    false,
    (char)UNKWNON_MODEL,
    0,
};
static bool oneWhite = false;

extern bool BTProcessLock;
extern int queueLength;

void setupBTTasksAndBLE();
bool checkIfIsTracker(char ch);
void hass_presence(JsonObject& HomePresence);
void BTforceScan();

void BTConfig_init() {
  BTConfig.bleConnect = AttemptBLEConnect;
  BTConfig.BLEinterval = TimeBtwRead;
  BTConfig.adaptiveScan = AdaptiveBLEScan;
  BTConfig.intervalActiveScan = TimeBtwActive;
  BTConfig.intervalConnect = TimeBtwConnect;
  BTConfig.scanDuration = Scan_duration;
  BTConfig.pubOnlySensors = PublishOnlySensors;
  BTConfig.pubRandomMACs = PublishRandomMACs;
  BTConfig.presenceEnable = HassPresence;
  BTConfig.presenceTopic = subjectHomePresence;
  BTConfig.presenceUseBeaconUuid = useBeaconUuidForPresence;
  BTConfig.minRssi = MinimumRSSI;
  BTConfig.extDecoderEnable = UseExtDecoder;
  BTConfig.extDecoderTopic = MQTTDecodeTopic;
  BTConfig.filterConnectable = BLE_FILTER_CONNECTABLE;
  BTConfig.pubAdvData = pubBLEAdvData;
  BTConfig.pubBeaconUuidForTopic = useBeaconUuidForTopic;
  BTConfig.ignoreWBlist = false;
  BTConfig.presenceAwayTimer = PresenceAwayTimer;
  BTConfig.movingTimer = MovingTimer;
  BTConfig.forcePassiveScan = false;
  BTConfig.enabled = EnableBT;
}

unsigned long timeBetweenConnect = 0;
unsigned long timeBetweenActive = 0;

String stateBTMeasures(bool start) {
  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
  JsonObject jo = jsonBuffer.to<JsonObject>();
  jo["bleconnect"] = BTConfig.bleConnect;
  jo["interval"] = BTConfig.BLEinterval;
  jo["adaptivescan"] = BTConfig.adaptiveScan;
  jo["intervalacts"] = BTConfig.intervalActiveScan;
  jo["intervalcnct"] = BTConfig.intervalConnect;
  jo["scanduration"] = BTConfig.scanDuration;
  jo["hasspresence"] = BTConfig.presenceEnable;
  jo["prestopic"] = BTConfig.presenceTopic;
  jo["presuseuuid"] = BTConfig.presenceUseBeaconUuid;
  jo["minrssi"] = -abs(BTConfig.minRssi); // Always export as negative value
  jo["extDecoderEnable"] = BTConfig.extDecoderEnable;
  jo["extDecoderTopic"] = BTConfig.extDecoderTopic;
  jo["pubuuid4topic"] = BTConfig.pubBeaconUuidForTopic;
  jo["ignoreWBlist"] = BTConfig.ignoreWBlist;
  jo["forcepscn"] = BTConfig.forcePassiveScan;
  jo["tskstck"] = uxTaskGetStackHighWaterMark(xProcBLETaskHandle);
  jo["crstck"] = uxTaskGetStackHighWaterMark(xCoreTaskHandle);
  jo["enabled"] = BTConfig.enabled;
  jo["scnct"] = scanCount;
#  if BLEDecoder
  jo["onlysensors"] = BTConfig.pubOnlySensors;
  jo["randommacs"] = BTConfig.pubRandomMACs;
  jo["filterConnectable"] = BTConfig.filterConnectable;
  jo["pubadvdata"] = BTConfig.pubAdvData;
  jo["presenceawaytimer"] = BTConfig.presenceAwayTimer;
  jo["movingtimer"] = BTConfig.movingTimer;
#  endif

  if (start) {
    THEENGS_LOG_NOTICE(F("BT sys: "));
    serializeJsonPretty(jsonBuffer, Serial);
    Serial.println();
    return ""; // Do not try to erase/write/send config at startup
  }
  String output;
  serializeJson(jo, output);
  jo["origin"] = subjectBTtoMQTT;
  enqueueJsonObject(jo, QueueSemaphoreTimeOutTask);
  return (output);
}

void BTConfig_fromJson(JsonObject& BTdata, bool startup = false) {
  // Attempts to connect to eligible devices or not
  Config_update(BTdata, "bleconnect", BTConfig.bleConnect);
  // Identify AdaptiveScan deactivation to pass to continuous mode or activation to come back to default settings
  if (startup == false) {
    if (BTdata.containsKey("hasspresence") && BTdata["hasspresence"] == false && BTConfig.presenceEnable == true) {
      BTdata["adaptivescan"] = true;
    } else if (BTdata.containsKey("hasspresence") && BTdata["hasspresence"] == true && BTConfig.presenceEnable == false) {
      BTdata["adaptivescan"] = false;
    }

    if (BTdata.containsKey("adaptivescan") && BTdata["adaptivescan"] == false && BTConfig.adaptiveScan == true) {
      BTdata["interval"] = MinTimeBtwScan;
      BTdata["intervalacts"] = MinTimeBtwScan;
      BTdata["scanduration"] = MinScanDuration;
    } else if (BTdata.containsKey("adaptivescan") && BTdata["adaptivescan"] == true && BTConfig.adaptiveScan == false) {
      BTdata["interval"] = TimeBtwRead;
      BTdata["intervalacts"] = TimeBtwActive;
      BTdata["scanduration"] = Scan_duration;
    }
    // Identify if the gateway is enabled or not and stop start accordingly
    if (BTdata.containsKey("enabled") && BTdata["enabled"] == false && BTConfig.enabled == true) {
      // Stop the gateway but without deinit to enable a future BT restart
      stopProcessing(false);
    } else if (BTdata.containsKey("enabled") && BTdata["enabled"] == true && BTConfig.enabled == false) {
      BTProcessLock = false;
      setupBTTasksAndBLE();
    }
  }
  // Home Assistant presence message
  Config_update(BTdata, "hasspresence", BTConfig.presenceEnable);
  // Time before before active scan
  // Scan interval set - and avoid intervalacts to be lower than interval
  if (BTdata.containsKey("interval") && BTdata["interval"] != 0) {
    BTConfig.adaptiveScan = false;
    Config_update(BTdata, "interval", BTConfig.BLEinterval);
    if (BTConfig.intervalActiveScan < BTConfig.BLEinterval) {
      Config_update(BTdata, "interval", BTConfig.intervalActiveScan);
    }
  }
  // Define if the scan is adaptive or not - and avoid intervalacts to be lower than interval
  if (BTdata.containsKey("intervalacts") && BTdata["intervalacts"] < BTConfig.BLEinterval) {
    BTConfig.adaptiveScan = false;
    // Config_update(BTdata, "interval", BTConfig.intervalActiveScan);
    BTConfig.intervalActiveScan = BTConfig.BLEinterval;
  } else {
    Config_update(BTdata, "intervalacts", BTConfig.intervalActiveScan);
  }
  //  Adaptive scan set
  Config_update(BTdata, "adaptivescan", BTConfig.adaptiveScan);
  // Time before a connect set
  Config_update(BTdata, "intervalcnct", BTConfig.intervalConnect);
  // publish all BLE devices discovered or  only the identified sensors (like temperature sensors)
  Config_update(BTdata, "scanduration", BTConfig.scanDuration);
  // define the duration for a scan; in milliseconds
  Config_update(BTdata, "onlysensors", BTConfig.pubOnlySensors);
  // publish devices which randomly change their MAC addresses
  Config_update(BTdata, "randommacs", BTConfig.pubRandomMACs);
  // Home Assistant presence message topic
  Config_update(BTdata, "prestopic", BTConfig.presenceTopic);
  // Home Assistant presence message use iBeacon UUID
  Config_update(BTdata, "presuseuuid", BTConfig.presenceUseBeaconUuid);
  // Timer to trigger a device state as offline if not seen
  Config_update(BTdata, "presenceawaytimer", BTConfig.presenceAwayTimer);
  // Timer to trigger a device state as offline if not seen
  Config_update(BTdata, "movingtimer", BTConfig.movingTimer);
  // Force passive scan
  Config_update(BTdata, "forcepscn", BTConfig.forcePassiveScan);
  // MinRSSI set
  Config_update(BTdata, "minrssi", BTConfig.minRssi);
  // Send undecoded device data
  Config_update(BTdata, "extDecoderEnable", BTConfig.extDecoderEnable);
  // Topic to send undecoded device data
  Config_update(BTdata, "extDecoderTopic", BTConfig.extDecoderTopic);
  // Sets whether to filter publishing
  Config_update(BTdata, "filterConnectable", BTConfig.filterConnectable);
  // Publish advertisement data
  Config_update(BTdata, "pubadvdata", BTConfig.pubAdvData);
  // Use iBeacon UUID as topic, instead of sender (random) MAC address
  Config_update(BTdata, "pubuuid4topic", BTConfig.pubBeaconUuidForTopic);
  // Disable Whitelist & Blacklist
  Config_update(BTdata, "ignoreWBlist", (BTConfig.ignoreWBlist));
  // Enable or disable the BT gateway
  Config_update(BTdata, "enabled", BTConfig.enabled);

  stateBTMeasures(startup);

  if (BTdata.containsKey("erase") && BTdata["erase"].as<bool>()) {
    // Erase config from NVS (non-volatile storage)
    preferences.begin(Gateway_Short_Name, false);
    if (preferences.isKey("BTConfig")) {
      int result = preferences.remove("BTConfig");
      THEENGS_LOG_NOTICE(F("BT config erase result: %d" CR), result);
      preferences.end();
      return; // Erase prevails on save, so skipping save
    } else {
      preferences.end();
      THEENGS_LOG_NOTICE(F("BT config not found" CR));
    }
  }

  if (BTdata.containsKey("save") && BTdata["save"].as<bool>()) {
    StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
    JsonObject jo = jsonBuffer.to<JsonObject>();
    jo["bleconnect"] = BTConfig.bleConnect;
    jo["interval"] = BTConfig.BLEinterval;
    jo["adaptivescan"] = BTConfig.adaptiveScan;
    jo["intervalacts"] = BTConfig.intervalActiveScan;
    jo["intervalcnct"] = BTConfig.intervalConnect;
    jo["scanduration"] = BTConfig.scanDuration;
    jo["onlysensors"] = BTConfig.pubOnlySensors;
    jo["randommacs"] = BTConfig.pubRandomMACs;
    jo["hasspresence"] = BTConfig.presenceEnable;
    jo["prestopic"] = BTConfig.presenceTopic;
    jo["presuseuuid"] = BTConfig.presenceUseBeaconUuid;
    jo["minrssi"] = -abs(BTConfig.minRssi); // Always export as negative value
    jo["extDecoderEnable"] = BTConfig.extDecoderEnable;
    jo["extDecoderTopic"] = BTConfig.extDecoderTopic;
    jo["filterConnectable"] = BTConfig.filterConnectable;
    jo["pubadvdata"] = BTConfig.pubAdvData;
    jo["pubuuid4topic"] = BTConfig.pubBeaconUuidForTopic;
    jo["ignoreWBlist"] = BTConfig.ignoreWBlist;
    jo["presenceawaytimer"] = BTConfig.presenceAwayTimer;
    jo["movingtimer"] = BTConfig.movingTimer;
    jo["forcepscn"] = BTConfig.forcePassiveScan;
    jo["enabled"] = BTConfig.enabled;
    // Save config into NVS (non-volatile storage)
    String conf = "";
    serializeJson(jsonBuffer, conf);
    preferences.begin(Gateway_Short_Name, false);
    int result = preferences.putString("BTConfig", conf);
    preferences.end();
    THEENGS_LOG_NOTICE(F("BT config save: %s, result: %d" CR), conf.c_str(), result);
  }
}

void BTConfig_load() {
  StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
  preferences.begin(Gateway_Short_Name, true);
  if (preferences.isKey("BTConfig")) {
    auto error = deserializeJson(jsonBuffer, preferences.getString("BTConfig", "{}"));
    preferences.end();
    THEENGS_LOG_NOTICE(F("BT config loaded" CR));
    if (error) {
      THEENGS_LOG_ERROR(F("BT config deserialization failed: %s, buffer capacity: %u" CR), error.c_str(), jsonBuffer.capacity());
      return;
    }
    if (jsonBuffer.isNull()) {
      THEENGS_LOG_WARNING(F("BT config is null" CR));
      return;
    }
    JsonObject jo = jsonBuffer.as<JsonObject>();
    BTConfig_fromJson(jo, true); // Never send MQTT message with config
    THEENGS_LOG_NOTICE(F("BT config loaded" CR));
  } else {
    preferences.end();
    THEENGS_LOG_NOTICE(F("BT config not found" CR));
  }
}

void PublishDeviceData(JsonObject& BLEdata);

atomic_int forceBTScan;

void createOrUpdateDevice(const char* mac, uint8_t flags, int model, int mac_type = 0, const char* name = "");

BLEdevice* getDeviceByMac(const char* mac); // Declared here to avoid pre-compilation issue (misplaced auto declaration by pio)
BLEdevice* getDeviceByMac(const char* mac) {
  THEENGS_LOG_TRACE(F("getDeviceByMac %s" CR), mac);

  for (vector<BLEdevice*>::iterator it = devices.begin(); it != devices.end(); ++it) {
    if ((strcmp((*it)->macAdr, mac) == 0)) {
      return *it;
    }
  }
  return &NO_BT_DEVICE_FOUND;
}

bool updateWorB(JsonObject& BTdata, bool isWhite) {
  THEENGS_LOG_TRACE(F("update WorB" CR));
  const char* jsonKey = isWhite ? "white-list" : "black-list";

  int size = BTdata[jsonKey].size();
  if (size == 0)
    return false;

  for (int i = 0; i < size; i++) {
    const char* mac = BTdata[jsonKey][i];
    createOrUpdateDevice(mac, (isWhite ? device_flags_isWhiteL : device_flags_isBlackL),
                         UNKWNON_MODEL);
  }

  return true;
}

void createOrUpdateDevice(const char* mac, uint8_t flags, int model, int mac_type, const char* name) {
  if (xSemaphoreTake(semaphoreCreateOrUpdateDevice, pdMS_TO_TICKS(30000)) == pdFALSE) {
    THEENGS_LOG_ERROR(F("Semaphore NOT taken" CR));
    return;
  }
  BLEdevice* device = getDeviceByMac(mac);
  if (device == &NO_BT_DEVICE_FOUND) {
    THEENGS_LOG_TRACE(F("add %s" CR), mac);
    //new device
    device = new BLEdevice();
    strcpy(device->macAdr, mac);
    device->isDisc = flags & device_flags_isDisc;
    device->isWhtL = flags & device_flags_isWhiteL;
    device->isBlkL = flags & device_flags_isBlackL;
    device->connect = flags & device_flags_connect;
    device->macType = mac_type;
    // Check name length
    if (strlen(name) > 20) {
      THEENGS_LOG_WARNING(F("Name too long, truncating" CR));
      strncpy(device->name, name, 20);
      device->name[19] = '\0';
    } else {
      strcpy(device->name, name);
    }
    device->sensorModel_id = model;
    device->lastUpdate = millis();
    devices.push_back(device);
    newDevices++;
  } else {
    THEENGS_LOG_TRACE(F("update %s" CR), mac);
    device->lastUpdate = millis();
    device->macType = mac_type;

    if (flags & device_flags_isDisc) {
      device->isDisc = true;
    }

    if (flags & device_flags_connect) {
      device->connect = true;
    }

    if (model != UNKWNON_MODEL && device->sensorModel_id == UNKWNON_MODEL) {
      newDevices++;
      device->isDisc = false;
      device->sensorModel_id = model;
    }

    // If a device has been added to the white-list, flag it so it can be auto-detected
    if (!device->isWhtL && flags & device_flags_isWhiteL) {
      newDevices++;
    }
    if (flags & device_flags_isWhiteL || flags & device_flags_isBlackL) {
      device->isWhtL = flags & device_flags_isWhiteL;
      device->isBlkL = flags & device_flags_isBlackL;
    }
  }

  // update oneWhite flag
  oneWhite = oneWhite || device->isWhtL;

  xSemaphoreGive(semaphoreCreateOrUpdateDevice);
}

void updateDevicesStatus() {
  for (vector<BLEdevice*>::iterator it = devices.begin(); it != devices.end(); ++it) {
    BLEdevice* p = *it;
    unsigned long now = millis();
    // Check for tracker status
    bool isTracker = false;
#  if BLEDecoder
    std::string tag = decoder.getTheengAttribute(p->sensorModel_id, "tag");
    if (tag.length() >= 4) {
      isTracker = checkIfIsTracker(tag[3]);
    }
    // Device tracker devices
    if (isTracker) { // We apply the offline status only for tracking device, can be extended further to all the devices
      if ((p->lastUpdate != 0) && (p->lastUpdate < (now - BTConfig.presenceAwayTimer) && (now > BTConfig.presenceAwayTimer)) &&
          (BTConfig.ignoreWBlist || ((!oneWhite || isWhite(p)) && !isBlack(p)))) { // Only if WBlist is disabled OR ((no white MAC OR this MAC is white) AND not a black listed MAC)) {
        StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
        JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
        BLEdata["id"] = p->macAdr;
        BLEdata["state"] = "offline";
        buildTopicFromId(BLEdata, subjectBTtoMQTT);
        enqueueJsonObject(BLEdata, QueueSemaphoreTimeOutTask);
        // We set the lastUpdate to 0 to avoid replublishing the offline state
        p->lastUpdate = 0;
      }
    }
    // Moving detection devices (devices with an accelerometer)
    if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BC08) {
      if ((p->lastUpdate != 0) && (p->lastUpdate < (now - BTConfig.movingTimer) && (now > BTConfig.movingTimer)) &&
          (BTConfig.ignoreWBlist || ((!oneWhite || isWhite(p)) && !isBlack(p)))) { // Only if WBlist is disabled OR ((no white MAC OR this MAC is white) AND not a black listed MAC)) {
        StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
        JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
        BLEdata["id"] = p->macAdr;
        BLEdata["state"] = "offline";
        buildTopicFromId(BLEdata, subjectBTtoMQTT);
        enqueueJsonObject(BLEdata, QueueSemaphoreTimeOutTask);
        // We set the lastUpdate to 0 to avoid replublishing the offline state
        p->lastUpdate = 0;
      }
    }
#  endif
  }
}

void dumpDevices() {
#  if LOG_LEVEL > LOG_LEVEL_NOTICE
  for (vector<BLEdevice*>::iterator it = devices.begin(); it != devices.end(); ++it) {
    BLEdevice* p = *it;
    THEENGS_LOG_TRACE(F("macAdr %s" CR), p->macAdr);
    THEENGS_LOG_TRACE(F("macType %d" CR), p->macType);
    THEENGS_LOG_TRACE(F("isDisc %d" CR), p->isDisc);
    THEENGS_LOG_TRACE(F("isWhtL %d" CR), p->isWhtL);
    THEENGS_LOG_TRACE(F("isBlkL %d" CR), p->isBlkL);
    THEENGS_LOG_TRACE(F("connect %d" CR), p->connect);
    THEENGS_LOG_TRACE(F("sensorModel_id %d" CR), p->sensorModel_id);
    THEENGS_LOG_TRACE(F("LastUpdate %u" CR), p->lastUpdate);
  }
#  endif
}

void strupp(char* beg) {
  while ((*beg = toupper(*beg)))
    ++beg;
}

#  ifdef ZmqttDiscovery
void DT24Discovery(const char* mac, const char* sensorModel_id) {
#    define DT24parametersCount 7
  THEENGS_LOG_TRACE(F("DT24Discovery" CR));
  const char* DT24sensor[DT24parametersCount][9] = {
      {HASS_TYPE_SENSOR, "volt", mac, HASS_CLASS_VOLTAGE, jsonVolt, "", "", HASS_UNIT_VOLT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "amp", mac, HASS_CLASS_CURRENT, jsonCurrent, "", "", HASS_UNIT_AMP, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "watt", mac, HASS_CLASS_POWER, jsonPower, "", "", "W", stateClassMeasurement},
      {HASS_TYPE_SENSOR, "watt-hour", mac, HASS_CLASS_POWER, jsonEnergy, "", "", HASS_UNIT_KWH, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "price", mac, "", jsonMsg, "", "", "", stateClassNone},
      {HASS_TYPE_SENSOR, "temp", mac, HASS_CLASS_TEMPERATURE, jsonTempc, "", "", HASS_UNIT_CELSIUS, stateClassMeasurement},
      {HASS_TYPE_BINARY_SENSOR, "inUse", mac, HASS_CLASS_POWER, jsonInuse, "", "", "", stateClassNone}
      //component type,name,availability topic,device class,value template,payload on, payload off, unit of measurement
  };

  createDiscoveryFromList(mac, DT24sensor, DT24parametersCount, "DT24", "ATorch", sensorModel_id);
}

void BM2Discovery(const char* mac, const char* sensorModel_id) {
#    define BM2parametersCount 2
  THEENGS_LOG_TRACE(F("BM2Discovery" CR));
  const char* BM2sensor[BM2parametersCount][9] = {
      {HASS_TYPE_SENSOR, "volt", mac, HASS_CLASS_VOLTAGE, jsonVoltBM, "", "", HASS_UNIT_VOLT, stateClassMeasurement}, // We use a json definition that retrieve only data from the BM decoder, as this sensor also advertizes volt as an iBeacon
      {HASS_TYPE_SENSOR, "batt", mac, HASS_CLASS_BATTERY, jsonBatt, "", "", HASS_UNIT_PERCENT, stateClassMeasurement}
      //component type,name,availability topic,device class,value template,payload on, payload off, unit of measurement
  };

  createDiscoveryFromList(mac, BM2sensor, BM2parametersCount, "BM2", "Generic", sensorModel_id);
}

void BM6Discovery(const char* mac, const char* sensorModel_id) {
#    define BM6parametersCount 3
  THEENGS_LOG_TRACE(F("BM6Discovery" CR));
  const char* BM6sensor[BM6parametersCount][9] = {
      {HASS_TYPE_SENSOR, "volt", mac, HASS_CLASS_VOLTAGE, jsonVoltBM, "", "", HASS_UNIT_VOLT, stateClassMeasurement}, // We use a json definition that retrieve only data from the BM decoder, as this sensor also advertizes volt as an iBeacon
      {HASS_TYPE_SENSOR, "temp", mac, HASS_CLASS_TEMPERATURE, jsonTempc, "", "", HASS_UNIT_CELSIUS, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "batt", mac, HASS_CLASS_BATTERY, jsonBatt, "", "", HASS_UNIT_PERCENT, stateClassMeasurement}
      //component type,name,availability topic,device class,value template,payload on, payload off, unit of measurement
  };

  createDiscoveryFromList(mac, BM6sensor, BM6parametersCount, "BM6", "Generic", sensorModel_id);
}

void LYWSD03MMCDiscovery(const char* mac, const char* sensorModel) {
#    define LYWSD03MMCparametersCount 4
  THEENGS_LOG_TRACE(F("LYWSD03MMCDiscovery" CR));
  const char* LYWSD03MMCsensor[LYWSD03MMCparametersCount][9] = {
      {HASS_TYPE_SENSOR, "batt", mac, HASS_CLASS_BATTERY, jsonBatt, "", "", HASS_UNIT_PERCENT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "volt", mac, "", jsonVolt, "", "", HASS_UNIT_VOLT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "temp", mac, HASS_CLASS_TEMPERATURE, jsonTempc, "", "", HASS_UNIT_CELSIUS, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "hum", mac, HASS_CLASS_HUMIDITY, jsonHum, "", "", HASS_UNIT_PERCENT, stateClassMeasurement}
      //component type,name,availability topic,device class,value template,payload on, payload off, unit of measurement
  };

  createDiscoveryFromList(mac, LYWSD03MMCsensor, LYWSD03MMCparametersCount, "LYWSD03MMC", "Xiaomi", sensorModel);
}

void MHO_C401Discovery(const char* mac, const char* sensorModel) {
#    define MHO_C401parametersCount 4
  THEENGS_LOG_TRACE(F("MHO_C401Discovery" CR));
  const char* MHO_C401sensor[MHO_C401parametersCount][9] = {
      {HASS_TYPE_SENSOR, "batt", mac, HASS_CLASS_BATTERY, jsonBatt, "", "", HASS_UNIT_PERCENT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "volt", mac, "", jsonVolt, "", "", HASS_UNIT_VOLT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "temp", mac, HASS_CLASS_TEMPERATURE, jsonTempc, "", "", HASS_UNIT_CELSIUS, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "hum", mac, HASS_CLASS_HUMIDITY, jsonHum, "", "", HASS_UNIT_PERCENT, stateClassMeasurement}
      //component type,name,availability topic,device class,value template,payload on, payload off, unit of measurement
  };

  createDiscoveryFromList(mac, MHO_C401sensor, MHO_C401parametersCount, "MHO_C401", "Xiaomi", sensorModel);
}

void HHCCJCY01HHCCDiscovery(const char* mac, const char* sensorModel) {
#    define HHCCJCY01HHCCparametersCount 5
  THEENGS_LOG_TRACE(F("HHCCJCY01HHCCDiscovery" CR));
  const char* HHCCJCY01HHCCsensor[HHCCJCY01HHCCparametersCount][9] = {
      {HASS_TYPE_SENSOR, "batt", mac, HASS_CLASS_BATTERY, jsonBatt, "", "", HASS_UNIT_PERCENT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "temp", mac, HASS_CLASS_TEMPERATURE, jsonTempc, "", "", HASS_UNIT_CELSIUS, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "lux", mac, HASS_CLASS_ILLUMINANCE, jsonLux, "", "", "lx", stateClassMeasurement},
      {HASS_TYPE_SENSOR, "fer", mac, "", jsonFer, "", "", "µS/cm", stateClassMeasurement},
      {HASS_TYPE_SENSOR, "moi", mac, "", jsonMoi, "", "", HASS_UNIT_PERCENT, stateClassMeasurement}
      //component type,name,availability topic,device class,value template,payload on, payload off, unit of measurement
  };

  createDiscoveryFromList(mac, HHCCJCY01HHCCsensor, HHCCJCY01HHCCparametersCount, "HHCCJCY01HHCC", "Xiaomi", sensorModel);
}

void XMWSDJ04MMCDiscovery(const char* mac, const char* sensorModel) {
#    define XMWSDJ04MMCparametersCount 4
  THEENGS_LOG_TRACE(F("XMWSDJ04MMCDiscovery" CR));
  const char* XMWSDJ04MMCsensor[XMWSDJ04MMCparametersCount][9] = {
      {HASS_TYPE_SENSOR, "batt", mac, HASS_CLASS_BATTERY, jsonBatt, "", "", HASS_UNIT_PERCENT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "volt", mac, "", jsonVolt, "", "", HASS_UNIT_VOLT, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "temp", mac, HASS_CLASS_TEMPERATURE, jsonTempc, "", "", HASS_UNIT_CELSIUS, stateClassMeasurement},
      {HASS_TYPE_SENSOR, "hum", mac, HASS_CLASS_HUMIDITY, jsonHum, "", "", HASS_UNIT_PERCENT, stateClassMeasurement}
      //component type,name,availability topic,device class,value template,payload on, payload off, unit of measurement
  };

  createDiscoveryFromList(mac, XMWSDJ04MMCsensor, XMWSDJ04MMCparametersCount, "XMWSDJ04MMC", "Xiaomi", sensorModel);
}

#  else
void LYWSD03MMCDiscovery(const char* mac, const char* sensorModel) {}
void MHO_C401Discovery(const char* mac, const char* sensorModel) {}
void HHCCJCY01HHCCDiscovery(const char* mac, const char* sensorModel) {}
void DT24Discovery(const char* mac, const char* sensorModel_id) {}
void BM2Discovery(const char* mac, const char* sensorModel_id) {}
void BM6Discovery(const char* mac, const char* sensorModel_id) {}
void XMWSDJ04MMCDiscovery(const char* mac, const char* sensorModel_id) {}
#  endif

/*
       Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleScan.cpp
       Ported to Arduino ESP32 by Evandro Copercini
    */
// core task implementation thanks to https://techtutorialsx.com/2017/05/09/esp32-running-code-on-a-specific-core/

//core on which the BLE detection task will run
static int taskCore = 0;

class ScanCallbacks : public NimBLEScanCallbacks {
  void onResult(const NimBLEAdvertisedDevice* advertisedDevice) {
    NimBLEAdvertisedDevice* ad = new NimBLEAdvertisedDevice(*advertisedDevice);
    if (xQueueSend(BLEQueue, &ad, 0) != pdTRUE) {
      THEENGS_LOG_ERROR(F("BLEQueue full" CR));
      delete (ad);
    }
  }
} scanCallbacks;

std::string convertServiceData(std::string deviceServiceData) {
  int serviceDataLength = (int)deviceServiceData.length();
  char spr[2 * serviceDataLength + 1];
  for (int i = 0; i < serviceDataLength; i++) sprintf(spr + 2 * i, "%.2x", (unsigned char)deviceServiceData[i]);
  spr[2 * serviceDataLength] = 0;
  THEENGS_LOG_TRACE(F("Converted service data (%d) to %s" CR), serviceDataLength, spr);
  return spr;
}

bool checkIfIsTracker(char ch) {
  uint8_t data = 0;
  if (ch >= '0' && ch <= '9')
    data = ch - '0';
  else if (ch >= 'a' && ch <= 'f')
    data = 10 + (ch - 'a');

  if (((data >> 3) & 0x01) == 1) {
    THEENGS_LOG_TRACE(F("Is Device Tracker" CR));
    return true;
  } else {
    return false;
  }
}

void procBLETask(void* pvParameters) {
  BLEAdvertisedDevice* advertisedDevice = nullptr;

  for (;;) {
    xQueueReceive(BLEQueue, &advertisedDevice, portMAX_DELAY);
    // Feed the watchdog
    //esp_task_wdt_reset();
    if (!BTProcessLock) {
      THEENGS_LOG_TRACE(F("Creating BLE buffer" CR));
      StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
      BLEdata["id"] = advertisedDevice->getAddress().toString();
      BLEdata["mac_type"] = advertisedDevice->getAddress().getType();
      BLEdata["adv_type"] = advertisedDevice->getAdvType();
      THEENGS_LOG_NOTICE(F("BT Device detected: %s" CR), BLEdata["id"].as<const char*>());
      BLEdevice* device = getDeviceByMac(BLEdata["id"].as<const char*>());

      if (BTConfig.filterConnectable && device->connect) {
        THEENGS_LOG_NOTICE(F("Filtered connectable device" CR));
        delete (advertisedDevice);
        continue;
      }

      if (BTConfig.ignoreWBlist || ((!oneWhite || isWhite(device)) && !isBlack(device))) { // Only if WBlist is disabled OR ((no white MAC OR this MAC is white) AND not a black listed MAC)
        if (advertisedDevice->haveName())
          BLEdata["name"] = (char*)advertisedDevice->getName().c_str();
        if (advertisedDevice->haveManufacturerData()) {
          BLEdata["manufacturerdata"] = NimBLEUtils::dataToHexString((uint8_t*)advertisedDevice->getManufacturerData().data(),
                                                                     advertisedDevice->getManufacturerData().length());
        }
        BLEdata["rssi"] = (int)advertisedDevice->getRSSI();
        if (advertisedDevice->haveTXPower())
          BLEdata["txpower"] = (int8_t)advertisedDevice->getTXPower();
        if (BTConfig.presenceEnable) {
          hass_presence(BLEdata); // with either only sensors or not we can use it for home assistant room presence component
        }
        if (advertisedDevice->haveServiceData()) {
          int serviceDataCount = advertisedDevice->getServiceDataCount();
          THEENGS_LOG_TRACE(F("Get services data number: %d" CR), serviceDataCount);
          for (int j = 0; j < serviceDataCount; j++) {
            StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBufferTemp;
            JsonObject BLEdataTemp = BLEdataBufferTemp.to<JsonObject>();
            BLEdataBufferTemp = BLEdataBuffer;
            std::string service_data = convertServiceData(advertisedDevice->getServiceData(j));
            THEENGS_LOG_TRACE(F("Service data: %s" CR), service_data.c_str());
            std::string serviceDatauuid = advertisedDevice->getServiceDataUUID(j).toString();
            THEENGS_LOG_TRACE(F("Service data UUID: %s" CR), (char*)serviceDatauuid.c_str());
            BLEdataTemp["servicedata"] = (char*)service_data.c_str();
            BLEdataTemp["servicedatauuid"] = (char*)serviceDatauuid.c_str();
            PublishDeviceData(BLEdataTemp);
          }
        } else {
          PublishDeviceData(BLEdata);
        }
      } else {
        THEENGS_LOG_TRACE(F("Filtered MAC device" CR));
      }
      updateDevicesStatus();
    }
    delete (advertisedDevice);
    vTaskDelay(10);
  }
}

/**
 * BLEscan used to retrieve BLE advertized data from devices without connection
 */
void BLEscan() {
  // Don't start the next scan until processing of previous results is complete.
  while (uxQueueMessagesWaiting(BLEQueue) || queueLength != 0) { // the criteria on queueLength could be adjusted to parallelize the scan and the queue processing
    delay(1); // Wait for queue to empty, a yield here instead of the delay cause the WDT to trigger
  }
  THEENGS_LOG_NOTICE(F("Scan begin" CR));
  BLEScan* pBLEScan = BLEDevice::getScan();
  pBLEScan->setScanCallbacks(&scanCallbacks);
  if ((millis() > (timeBetweenActive + BTConfig.intervalActiveScan) || BTConfig.intervalActiveScan == BTConfig.BLEinterval) && !BTConfig.forcePassiveScan) {
    pBLEScan->setActiveScan(true);
    timeBetweenActive = millis();
  } else {
    pBLEScan->setActiveScan(false);
  }
  pBLEScan->setInterval(BLEScanInterval);
  pBLEScan->setWindow(BLEScanWindow);
  NimBLEScanResults foundDevices = pBLEScan->getResults(BTConfig.scanDuration, false);
  if (foundDevices.getCount())
    scanCount++;
  THEENGS_LOG_NOTICE(F("Found %d devices, scan number %d end" CR), foundDevices.getCount(), scanCount);
  THEENGS_LOG_TRACE(F("Process BLE stack free: %u" CR), uxTaskGetStackHighWaterMark(xProcBLETaskHandle));
}

/**
 * Connect to BLE devices and initiate the callbacks with a service/characteristic request
 */
#  if BLEDecoder
void BLEconnect() {
  if (!BTProcessLock) {
    THEENGS_LOG_NOTICE(F("BLE Connect begin" CR));
    do {
      for (vector<BLEdevice*>::iterator it = devices.begin(); it != devices.end(); ++it) {
        BLEdevice* p = *it;
        if (p->connect) {
          THEENGS_LOG_TRACE(F("Model to connect found: %s" CR), p->macAdr);
          NimBLEAddress addr((const char*)p->macAdr, p->macType);
          if (p->sensorModel_id == BLEconectable::id::LYWSD03MMC ||
              p->sensorModel_id == BLEconectable::id::MHO_C401) {
            LYWSD03MMC_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
            BLEclient.publishData();
          } else if (p->sensorModel_id == BLEconectable::id::DT24_BLE) {
            DT24_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
            BLEclient.publishData();
          } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BM2) {
            BM2_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
            BLEclient.publishData();
          } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BM6) {
            BM6_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
            BLEclient.publishData();
          } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::HHCCJCY01HHCC) {
            HHCCJCY01HHCC_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
            BLEclient.publishData();
          } else if (p->sensorModel_id == BLEconectable::id::XMWSDJ04MMC) {
            XMWSDJ04MMC_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
            BLEclient.publishData();
          } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::SBS1) {
            SBS1_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
          } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::SBBT) {
            SBBT_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
          } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::SBCU) {
            SBCU_connect BLEclient(addr);
            BLEclient.processActions(BLEactions);
          } else {
            GENERIC_connect BLEclient(addr);
            if (BLEclient.processActions(BLEactions)) {
              // If we don't regularly connect to this, disable connections so advertisements
              // won't be filtered if BLE_FILTER_CONNECTABLE is set.
              p->connect = false;
            }
          }
          if (BLEactions.size() > 0) {
            std::vector<BLEAction> swap;
            for (auto& it : BLEactions) {
              if (!it.complete && --it.ttl) {
                swap.push_back(it);
              } else if (it.addr == NimBLEAddress(p->macAdr, p->macType)) {
                if (p->sensorModel_id != BLEconectable::id::DT24_BLE &&
                    p->sensorModel_id != TheengsDecoder::BLE_ID_NUM::HHCCJCY01HHCC &&
                    p->sensorModel_id != BLEconectable::id::LYWSD03MMC &&
                    p->sensorModel_id != TheengsDecoder::BLE_ID_NUM::BM2 &&
                    p->sensorModel_id != TheengsDecoder::BLE_ID_NUM::BM6 &&
                    p->sensorModel_id != BLEconectable::id::MHO_C401 &&
                    p->sensorModel_id != BLEconectable::id::XMWSDJ04MMC) {
                  // if irregulary connected to and connection failed clear the connect flag.
                  p->connect = false;
                }
              }
            }
            std::swap(BLEactions, swap);
          }
        }
      }
    } while (BLEactions.size() > 0);
    THEENGS_LOG_NOTICE(F("BLE Connect end" CR));
  }
}
#  else
void BLEconnect() {}
#  endif

void stopProcessing(bool deinit) {
  if (BTConfig.enabled) {
    BTProcessLock = true;
    // We stop the scan
    THEENGS_LOG_NOTICE(F("Stopping BLE scan" CR));
    BLEScan* pBLEScan = BLEDevice::getScan();
    if (pBLEScan->isScanning()) {
      pBLEScan->stop();
    }

    if (xSemaphoreTake(semaphoreBLEOperation, pdMS_TO_TICKS(5000)) == pdTRUE) {
      THEENGS_LOG_NOTICE(F("Stopping BLE tasks" CR));
      //Suspending, deleting tasks and stopping BT to free memory
      vTaskSuspend(xCoreTaskHandle);
      vTaskDelete(xCoreTaskHandle);
      vTaskSuspend(xProcBLETaskHandle);
      vTaskDelete(xProcBLETaskHandle);
      xSemaphoreGive(semaphoreBLEOperation);
    }
    // Using deinit to free memory, should only be used if we are going to restart the gateway
    if (deinit)
      BLEDevice::deinit(true);
  }
  THEENGS_LOG_NOTICE(F("BLE gateway stopped, free heap: %d" CR), ESP.getFreeHeap());
}

void coreTask(void* pvParameters) {
  while (true) {
    if (!BTProcessLock) {
      if (xSemaphoreTake(semaphoreBLEOperation, pdMS_TO_TICKS(30000)) == pdTRUE) {
        BLEscan();
        // Launching a connect every TimeBtwConnect
        if (millis() > (timeBetweenConnect + BTConfig.intervalConnect) && BTConfig.bleConnect) {
          timeBetweenConnect = millis();
          BLEconnect();
        }
        //dumpDevices();
        THEENGS_LOG_TRACE(F("CoreTask stack free: %u" CR), uxTaskGetStackHighWaterMark(xCoreTaskHandle));
        xSemaphoreGive(semaphoreBLEOperation);
      } else {
        THEENGS_LOG_ERROR(F("Failed to start scan - BLE busy" CR));
      }
      if (SYSConfig.powerMode > 0) {
        int scan = atomic_exchange_explicit(&forceBTScan, 0, ::memory_order_seq_cst); // is this enough, it will wait the full deepsleep...
        if (scan == 1) BTforceScan();
        ready_to_sleep = true;
      } else {
        for (int interval = BTConfig.BLEinterval, waitms; interval > 0; interval -= waitms) {
          int scan = atomic_exchange_explicit(&forceBTScan, 0, ::memory_order_seq_cst);
          if (scan == 1) BTforceScan(); // should we break after this?
          delay(waitms = interval > 100 ? 100 : interval); // 100ms
        }
      }
    }
    delay(1);
  }
}

void setupBTTasksAndBLE() {
#  ifdef CONFIG_BTDM_BLE_SCAN_DUPL
  BLEDevice::setScanDuplicateCacheSize(BLEScanDuplicateCacheSize);
#  endif
  BLEDevice::init("");
  xTaskCreateUniversal(
      procBLETask, /* Function to implement the task */
      "procBLETask", /* Name of the task */
#  if defined(USE_ESP_IDF) || defined(USE_BLUFI)
      14500,
#  else
      9500, /* Stack size in bytes */
#  endif
      NULL, /* Task input parameter */
      2, /* Priority of the task (set higher than core task) */
      &xProcBLETaskHandle, /* Task handle. */
      1); /* Core where the task should run */

  // we setup a task with priority one to avoid conflict with other gateways
  xTaskCreateUniversal(
      coreTask, /* Function to implement the task */
      "coreTask", /* Name of the task */
      5120, /* Stack size in bytes */
      NULL, /* Task input parameter */
      1, /* Priority of the task */
      &xCoreTaskHandle, /* Task handle. */
      taskCore); /* Core where the task should run */
}

void setupBT() {
  BTConfig_init();
  BTConfig_load();
  THEENGS_LOG_NOTICE(F("BLE scans interval: %d" CR), BTConfig.BLEinterval);
  THEENGS_LOG_NOTICE(F("BLE connects interval: %d" CR), BTConfig.intervalConnect);
  THEENGS_LOG_NOTICE(F("BLE scan duration: %d" CR), BTConfig.scanDuration);
  THEENGS_LOG_NOTICE(F("Publishing only BLE sensors: %T" CR), BTConfig.pubOnlySensors);
  THEENGS_LOG_NOTICE(F("Publishing random MAC devices: %T" CR), BTConfig.pubRandomMACs);
  THEENGS_LOG_NOTICE(F("Adaptive BLE scan: %T" CR), BTConfig.adaptiveScan);
  THEENGS_LOG_NOTICE(F("Active BLE scan interval: %d" CR), BTConfig.intervalActiveScan);
  THEENGS_LOG_NOTICE(F("minrssi: %d" CR), -abs(BTConfig.minRssi));
  THEENGS_LOG_NOTICE(F("Presence Away Timer: %d" CR), BTConfig.presenceAwayTimer);
  THEENGS_LOG_NOTICE(F("Moving Timer: %d" CR), BTConfig.movingTimer);
  THEENGS_LOG_NOTICE(F("Force passive scan: %T" CR), BTConfig.forcePassiveScan);
  THEENGS_LOG_NOTICE(F("Enabled BLE: %T" CR), BTConfig.enabled);

  atomic_init(&forceBTScan, 0); // in theory, we don't need this

  semaphoreCreateOrUpdateDevice = xSemaphoreCreateBinary();
  xSemaphoreGive(semaphoreCreateOrUpdateDevice);

  semaphoreBLEOperation = xSemaphoreCreateBinary();
  xSemaphoreGive(semaphoreBLEOperation);

  BLEQueue = xQueueCreate(QueueSize, sizeof(NimBLEAdvertisedDevice*));
  if (BTConfig.enabled) {
    setupBTTasksAndBLE();
    THEENGS_LOG_NOTICE(F("gatewayBT multicore ESP32 setup done" CR));
  } else {
    THEENGS_LOG_NOTICE(F("gatewayBT multicore ESP32 setup disabled" CR));
  }
}

boolean valid_service_data(const char* data, int size) {
  for (int i = 0; i < size; ++i) {
    if (data[i] != 48) // 48 correspond to 0 in ASCII table
      return true;
  }
  return false;
}

#  if defined(ZmqttDiscovery) && BLEDecoder == true
// This function always should be called from the main core as it generates direct mqtt messages
// When overrideDiscovery=true, we publish discovery messages of known devices (even if no new)
void launchBTDiscovery(bool overrideDiscovery) {
  if (!overrideDiscovery && newDevices == 0)
    return;
  if (xSemaphoreTake(semaphoreCreateOrUpdateDevice, pdMS_TO_TICKS(QueueSemaphoreTimeOutTask)) == pdFALSE) {
    THEENGS_LOG_ERROR(F("Semaphore NOT taken" CR));
    return;
  }
  newDevices = 0;
  vector<BLEdevice*> localDevices = devices;
  xSemaphoreGive(semaphoreCreateOrUpdateDevice);
  for (vector<BLEdevice*>::iterator it = localDevices.begin(); it != localDevices.end(); ++it) {
    BLEdevice* p = *it;
    THEENGS_LOG_TRACE(F("Device mac %s" CR), p->macAdr);
    // Do not launch discovery for the devices already discovered (unless we have overrideDiscovery) or that are not unique by their MAC Address (iBeacon, GAEN and Microsoft CDP)
    if (overrideDiscovery || !isDiscovered(p)) {
      String macWOdots = String(p->macAdr);
      macWOdots.replace(":", "");
      if (p->sensorModel_id >= 0) {
        THEENGS_LOG_TRACE(F("Looking for Model_id: %d" CR), p->sensorModel_id);
        std::string properties = decoder.getTheengProperties(p->sensorModel_id);
        THEENGS_LOG_TRACE(F("properties: %s" CR), properties.c_str());
        std::string brand = decoder.getTheengAttribute(p->sensorModel_id, "brand");
        std::string model = decoder.getTheengAttribute(p->sensorModel_id, "model");
#    if ForceDeviceName
        if (p->name[0] != '\0') {
          model = p->name;
        }
#    endif
        std::string model_id = decoder.getTheengAttribute(p->sensorModel_id, "model_id");

        // Check for tracker status
        bool isTracker = false;
        std::string tag = decoder.getTheengAttribute(p->sensorModel_id, "tag");
        if (tag.length() >= 4) {
          isTracker = checkIfIsTracker(tag[3]);
        }

        String discovery_topic = String(subjectBTtoMQTT) + "/" + macWOdots;
        if (!BTConfig.extDecoderEnable && // Do not decode if an external decoder is configured
            p->sensorModel_id > UNKWNON_MODEL &&
            p->sensorModel_id < TheengsDecoder::BLE_ID_NUM::BLE_ID_MAX &&
            p->sensorModel_id != TheengsDecoder::BLE_ID_NUM::HHCCJCY01HHCC && 
            p->sensorModel_id != TheengsDecoder::BLE_ID_NUM::BM2 &&
            p->sensorModel_id != TheengsDecoder::BLE_ID_NUM::BM6) { // Exception on HHCCJCY01HHCC and BM2/BM6 as these ones are discoverable and connectable
          if (isTracker) {
            String tracker_name = String(model_id.c_str()) + "-tracker";
            String tracker_id = macWOdots + "-tracker";
            createDiscovery(HASS_TYPE_DEVICE_TRACKER,
                            discovery_topic.c_str(), tracker_name.c_str(), tracker_id.c_str(),
                            will_Topic, "occupancy", "{% if value_json.get('rssi') -%}home{%- else -%}not_home{%- endif %}",
                            "", "", "",
                            0, "", "", false, "",
                            model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                            stateClassNone);
          }
          if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BC08) {
            String sensor_name = String(model_id.c_str()) + "-moving";
            String sensor_id = macWOdots + "-moving";
            createDiscovery(HASS_TYPE_BINARY_SENSOR,
                            discovery_topic.c_str(), sensor_name.c_str(), sensor_id.c_str(),
                            will_Topic, "moving", "{% if value_json.get('accx') -%}on{%- else -%}off{%- endif %}",
                            "on", "off", "",
                            0, "", "", false, "",
                            model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                            stateClassNone);
          }
          if (!properties.empty()) {
            StaticJsonDocument<JSON_MSG_BUFFER> jsonBuffer;
            auto error = deserializeJson(jsonBuffer, properties);
            if (error) {
              if (jsonBuffer.overflowed()) {
                // This should not happen if JSON_MSG_BUFFER is large enough for
                // the Theengs json properties
                THEENGS_LOG_ERROR(F("JSON deserialization of Theengs properties overflowed (error %s), buffer capacity: %u. Program might crash. Properties json: %s" CR),
                          error.c_str(), jsonBuffer.capacity(), properties.c_str());
              } else {
                THEENGS_LOG_ERROR(F("JSON deserialization of Theengs properties errored: %" CR),
                          error.c_str());
              }
            }
            for (JsonPair prop : jsonBuffer["properties"].as<JsonObject>()) {
              THEENGS_LOG_TRACE(F("Key: %s"), prop.key().c_str());
              THEENGS_LOG_TRACE(F("Unit: %s"), prop.value()["unit"].as<const char*>());
              THEENGS_LOG_TRACE(F("Name: %s"), prop.value()["name"].as<const char*>());
              String entity_name = String(model_id.c_str()) + "-" + String(prop.key().c_str());
              String unique_id = macWOdots + "-" + String(prop.key().c_str());
              String value_template = "{{ value_json." + String(prop.key().c_str()) + " | is_defined }}";
              if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::SBS1 && strcmp(prop.key().c_str(), "state") == 0) {
                String payload_on = "{\"model_id\":\"X1\",\"cmd\":\"on\",\"id\":\"" + String(p->macAdr) + "\"}";
                String payload_off = "{\"model_id\":\"X1\",\"cmd\":\"off\",\"id\":\"" + String(p->macAdr) + "\"}";
                createDiscovery(HASS_TYPE_SWITCH, //set Type
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, HASS_TYPE_SWITCH, value_template.c_str(),
                                payload_on.c_str(), payload_off.c_str(), "", 0,
                                Gateway_AnnouncementMsg, will_Message, false, subjectMQTTtoBT,
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassNone, "off", "on");
                unique_id = macWOdots + "-press";
                entity_name = String(model_id.c_str()) + "-press";
                String payload_press = "{\"model_id\":\"X1\",\"cmd\":\"press\",\"id\":\"" + String(p->macAdr) + "\"}";
                createDiscovery(HASS_TYPE_BUTTON, //set Type
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, HASS_TYPE_BUTTON, "",
                                payload_press.c_str(), "", "", //set,payload_on,payload_off,unit_of_meas,
                                0, //set  off_delay
                                Gateway_AnnouncementMsg, will_Message, false, subjectMQTTtoBT,
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassNone);
              } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::SBBT && strcmp(prop.key().c_str(), "open") == 0) {
                value_template = "{% if value_json.direction == \"up\" -%} {{ 100 - value_json.open/2 }}{% elif value_json.direction == \"down\" %}{{ value_json.open/2 }}{% else %} {{ value_json.open/2 }}{%- endif %}";
                String command_template = "{\"model_id\":\"W270160X\",\"tilt\":{{ value | int }},\"id\":\"" + String(p->macAdr) + "\"}";
                createDiscovery(HASS_TYPE_COVER, //set Type
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, HASS_TYPE_COVER, value_template.c_str(),
                                "50", "", "", 0,
                                Gateway_AnnouncementMsg, will_Message, false, subjectMQTTtoBT,
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                "blind", nullptr, nullptr, nullptr, command_template.c_str());
              } else if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::SBCU && strcmp(prop.key().c_str(), "position") == 0) {
                String command_template = "{\"model_id\":\"W070160X\",\"position\":{{ value | int }},\"id\":\"" + String(p->macAdr) + "\"}";
                createDiscovery(HASS_TYPE_COVER, //set Type
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, HASS_TYPE_COVER, "{{ value_json.position }}",
                                "0", "100", "", 0,
                                Gateway_AnnouncementMsg, will_Message, false, subjectMQTTtoBT,
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                "curtain", nullptr, nullptr, nullptr, command_template.c_str());
              } else if ((p->sensorModel_id == TheengsDecoder::XMTZC04HMKG || p->sensorModel_id == TheengsDecoder::XMTZC04HMLB || p->sensorModel_id == TheengsDecoder::XMTZC05HMKG || p->sensorModel_id == TheengsDecoder::XMTZC05HMLB) &&
                         strcmp(prop.key().c_str(), "weighing_mode") == 0) {
                createDiscovery(HASS_TYPE_SENSOR,
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, "enum", value_template.c_str(),
                                "", "", prop.value()["unit"],
                                0, "", "", false, "",
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassMeasurement, nullptr, nullptr, "[\"person\",\"object\"]");
              } else if ((p->sensorModel_id == TheengsDecoder::XMTZC04HMKG || p->sensorModel_id == TheengsDecoder::XMTZC04HMLB || p->sensorModel_id == TheengsDecoder::XMTZC05HMKG || p->sensorModel_id == TheengsDecoder::XMTZC05HMLB) &&
                         strcmp(prop.key().c_str(), "unit") == 0) {
                createDiscovery(HASS_TYPE_SENSOR,
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, "enum", value_template.c_str(),
                                "", "", prop.value()["unit"],
                                0, "", "", false, "",
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassMeasurement, nullptr, nullptr, "[\"lb\",\"kg\",\"jin\"]");
              } else if (strcmp(prop.value()["unit"], "string") == 0 && strcmp(prop.key().c_str(), "mac") != 0) {
                createDiscovery(HASS_TYPE_SENSOR,
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, prop.value()["name"], value_template.c_str(),
                                "", "", "",
                                0, "", "", false, "",
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassNone);
              } else if (p->sensorModel_id == TheengsDecoder::MUE4094RT && strcmp(prop.value()["unit"], "status") == 0) { // This device does not a broadcast when there is nothing detected so adding a timeout
                createDiscovery(HASS_TYPE_BINARY_SENSOR,
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, prop.value()["name"], value_template.c_str(),
                                "True", "False", "",
                                BTConfig.presenceAwayTimer / 1000, "", "", false, "",
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassNone);
              } else if (strcmp(prop.value()["unit"], "status") == 0) {
                createDiscovery(HASS_TYPE_BINARY_SENSOR,
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, prop.value()["name"], value_template.c_str(),
                                "True", "False", "",
                                0, "", "", false, "",
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassNone);
              } else if (strcmp(prop.key().c_str(), "device") != 0 && strcmp(prop.key().c_str(), "mac") != 0) { // Exception on device and mac as these ones are not sensors
                createDiscovery(HASS_TYPE_SENSOR,
                                discovery_topic.c_str(), entity_name.c_str(), unique_id.c_str(),
                                will_Topic, prop.value()["name"], value_template.c_str(),
                                "", "", prop.value()["unit"],
                                0, "", "", false, "",
                                model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                                stateClassMeasurement);
              }
            }
          }

          String rssi_name = String(model_id.c_str()) + "-rssi"; // rssi diagnostic entity_category
          String rssi_id = macWOdots + "-rssi";
          createDiscovery(HASS_TYPE_SENSOR,
                          discovery_topic.c_str(), rssi_name.c_str(), rssi_id.c_str(),
                          will_Topic, "signal_strength", jsonRSSI,
                          "", "", "dB",
                          0, "", "", false, "",
                          model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                          stateClassMeasurement, nullptr, nullptr, nullptr, nullptr, true);

        } else {
          if ((p->sensorModel_id > BLEconectable::id::MIN &&
               p->sensorModel_id < BLEconectable::id::MAX) ||
              p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::HHCCJCY01HHCC || p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BM2 || p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BM6) {
            // Discovery of sensors from which we retrieve data only by connect
            if (p->sensorModel_id == BLEconectable::id::DT24_BLE) {
              DT24Discovery(macWOdots.c_str(), "DT24-BLE");
            }
            if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BM2) {
              // Sensor discovery
              BM2Discovery(macWOdots.c_str(), "BM2");
              // Device tracker discovery
              String tracker_id = macWOdots + "-tracker";
              createDiscovery(HASS_TYPE_DEVICE_TRACKER,
                              discovery_topic.c_str(), "BM2-tracker", tracker_id.c_str(),
                              will_Topic, "occupancy", "{% if value_json.get('rssi') -%}home{%- else -%}not_home{%- endif %}",
                              "", "", "",
                              0, "", "", false, "",
                              model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                              stateClassNone);
            }
            if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::BM6) {
              // Sensor discovery
              BM6Discovery(macWOdots.c_str(), "BM6");
              // Device tracker discovery
              String tracker_id = macWOdots + "-tracker";
              createDiscovery(HASS_TYPE_DEVICE_TRACKER,
                              discovery_topic.c_str(), "BM6-tracker", tracker_id.c_str(),
                              will_Topic, "occupancy", "{% if value_json.get('rssi') -%}home{%- else -%}not_home{%- endif %}",
                              "", "", "",
                              0, "", "", false, "",
                              model.c_str(), brand.c_str(), model_id.c_str(), macWOdots.c_str(), false,
                              stateClassNone);
            }
            if (p->sensorModel_id == BLEconectable::id::LYWSD03MMC) {
              LYWSD03MMCDiscovery(macWOdots.c_str(), "LYWSD03MMC");
            }
            if (p->sensorModel_id == BLEconectable::id::MHO_C401) {
              MHO_C401Discovery(macWOdots.c_str(), "MHO-C401");
            }
            if (p->sensorModel_id == BLEconectable::id::XMWSDJ04MMC) {
              XMWSDJ04MMCDiscovery(macWOdots.c_str(), "XMWSDJ04MMC");
            }
            if (p->sensorModel_id == TheengsDecoder::BLE_ID_NUM::HHCCJCY01HHCC) {
              HHCCJCY01HHCCDiscovery(macWOdots.c_str(), "HHCCJCY01HHCC");
            }
          } else {
            THEENGS_LOG_TRACE(F("Device UNKNOWN_MODEL %s" CR), p->macAdr);
          }
        }
      }
      p->isDisc = true; // we don't need the semaphore and all the search magic via createOrUpdateDevice
    } else {
      THEENGS_LOG_TRACE(F("Device already discovered or that doesn't require discovery %s" CR), p->macAdr);
    }
  }
}
#  else
void launchBTDiscovery(bool overrideDiscovery) {}
#  endif

#  if BLEDecryptor
// ** TODO - Hex string to bytes, there is probably a function for this already just need to find it
int hexToBytes(String hex, uint8_t* out, size_t maxLen) {
  int len = hex.length();
  int bytesToWrite = min(len / 2, (int)maxLen);
  if (len % 2) return -1; // Odd length is invalid
  for (int i = 0, j = 0; j < bytesToWrite; i += 2, j++) {
    out[j] = (uint8_t)strtol(hex.substring(i, i + 2).c_str(), nullptr, 16);
  }
  return bytesToWrite;
}
// Reverse bytes
void reverseBytes(uint8_t* data, size_t length) {
  size_t i;
  for (i = 0; i < length / 2; i++) {
    uint8_t temp = data[i];
    data[i] = data[length - 1 - i];
    data[length - 1 - i] = temp;
  }
}
#  endif

#  if BLEDecoder
void process_bledata(JsonObject& BLEdata) {
  yield(); // Necessary to let the loop run in case of connectivity issues
  if (!BLEdata.containsKey("id")) {
    THEENGS_LOG_ERROR(F("No mac address in the payload" CR));
    return;
  }
  const char* mac = BLEdata["id"].as<const char*>();
  THEENGS_LOG_TRACE(F("Processing BLE data %s" CR), BLEdata["id"].as<const char*>());
  int model_id = BTConfig.extDecoderEnable ? -1 : decoder.decodeBLEJson(BLEdata);
  int mac_type = BLEdata["mac_type"].as<int>();

#    if BLEDecryptor
  if (BLEdata["encr"] && (BLEdata["encr"].as<int>() > 0 && BLEdata["encr"].as<int>() <= 3)) {
    // Decrypting Encrypted BLE Data PVVX, BTHome or Victron
    THEENGS_LOG_TRACE(F("[BLEDecryptor] Decrypt ENCR:%d ModelID:%s Payload:%s" CR), BLEdata["encr"].as<int>(), BLEdata["model_id"].as<const char*>(), BLEdata["cipher"].as<const char*>());

    // MAC address
    String macWOdots = BLEdata["id"].as<String>(); // Mac Address without dots
    macWOdots.replace(":", "");
    unsigned char macAddress[6];
    int maclen = hexToBytes(macWOdots, macAddress, 6);
    if (maclen != 6) {
      THEENGS_LOG_ERROR(F("[BLEDecryptor] Invalid MAC Address length %d" CR), maclen);
      return;
    }

    // AES decryption key
    unsigned char bleaeskey[16];
    int bleaeskeylength = 0;
    if (ble_aes_keys.containsKey(macWOdots)) {
      THEENGS_LOG_TRACE(F("[BLEDecryptor] Custom AES key %s" CR), ble_aes_keys[macWOdots].as<const char*>());
      bleaeskeylength = hexToBytes(ble_aes_keys[macWOdots], bleaeskey, 16);
    } else {
      THEENGS_LOG_TRACE(F("[BLEDecryptor] Default AES key" CR));
      bleaeskeylength = hexToBytes(ble_aes, bleaeskey, 16);
    }
    // Check AES Key
    if (bleaeskeylength != 16) {
      THEENGS_LOG_ERROR(F("[BLEDecryptor] Invalid key length %d" CR), bleaeskeylength);
      return;
    }

    // Build nonce and aad
    uint8_t nonce[16];
    int noncelength = 0;
    unsigned char aad[1];
    int aadLength;

    if (BLEdata["encr"].as<int>() == 1) { // PVVX Encrypted
      noncelength = 11; // 11 bytes
      reverseBytes(macAddress, 6); // 6 bytes: device address in reverse
      memcpy(nonce, macAddress, 6);
      int maclen = hexToBytes(macWOdots, macAddress, 6);

      unsigned char servicedata[16];
      int servicedatalen = hexToBytes(BLEdata["servicedata"].as<String>(), servicedata, 16);
      nonce[6] = servicedatalen + 3; // 1 byte : length of (service data + type and UUID)
      nonce[7] = 0x16; // 1 byte : "16" -> AD type for "Service Data - 16-bit UUID"
      nonce[8] = 0x1A; // 2 bytes: "1a18" -> UUID 181a in little-endian
      nonce[9] = 0x18; //
      unsigned char ctr[1]; // 1 byte : counter
      int ctrlen = hexToBytes(BLEdata["ctr"].as<String>(), ctr, 1);
      if (ctrlen != 1) {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Invalid counter length %d" CR), ctrlen);
        return;
      }
      nonce[10] = ctr[0];
      aad[0] = 0x11;
      aadLength = 1;
      THEENGS_LOG_TRACE(F("[BLEDecryptor] PVVX nonce %s" CR), NimBLEUtils::dataToHexString(nonce, noncelength).c_str());

    } else if (BLEdata["encr"].as<int>() == 2) { // BTHome V2 Encrypted
      noncelength = 13; // 13 bytes
      memcpy(nonce, macAddress, 6);
      nonce[6] = 0xD2; // UUID
      nonce[7] = 0xFC;
      nonce[8] = 0x41; // BTHome Device Data encrypted payload byte
      unsigned char ctr[4]; // Counter
      int ctrlen = hexToBytes(BLEdata["ctr"].as<String>(), ctr, 4);
      if (ctrlen != 4) {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Invalid counter length %d" CR), ctrlen);
        return;
      }
      memcpy(&nonce[9], ctr, 4);
      aad[0] = 0x00;
      aadLength = 0;
      THEENGS_LOG_TRACE(F("[BLEDecryptor] BTHomeV2 nonce %s" CR), NimBLEUtils::dataToHexString(nonce, noncelength).c_str());

    } else if (BLEdata["encr"].as<int>() == 3) {
      nonce[16] = {0}; // Victron has a 16 byte zero padded nonce with IV bytes 6,7
      unsigned char iv[2];
      int ivlen = hexToBytes(BLEdata["ctr"].as<String>(), iv, 2);
      if (ivlen != 2) {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Invalid iv length %d" CR), ivlen);
        return;
      }
      memcpy(nonce, iv, 2);
      memset(nonce + 2, 0, 14); // 14 bytes: zero padding
      THEENGS_LOG_TRACE(F("[BLEDecryptor] Victron nonce %s" CR), NimBLEUtils::dataToHexString(nonce, 16).c_str());
    } else {
      return; // No match
    }

    // Ciphertext to bytes
    String cipherHex = BLEdata["cipher"].as<String>();
    int cipherlen = cipherHex.length() / 2; // Number of bytes in ciphertext
    unsigned char ciphertext[cipherlen];
    int ciphertextlen = hexToBytes(BLEdata["cipher"].as<String>(), ciphertext, cipherlen);
    unsigned char decrypted[cipherlen];

    // Decrypt ciphertext
    if (BLEdata["encr"].as<int>() == 1 || BLEdata["encr"].as<int>() == 2) {
      // Decrypt PVVX and BTHome V2 ciphertext using AES CCM
      mbedtls_ccm_context ctx;
      mbedtls_ccm_init(&ctx);
      if (mbedtls_ccm_setkey(&ctx, MBEDTLS_CIPHER_ID_AES, bleaeskey, 128) != 0) {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Failed to set AES key to mbedtls" CR));
        return;
      }

      // Message Integrity Check (MIC)
      unsigned char mic[4];
      int miclen = hexToBytes(BLEdata["mic"].as<String>(), mic, 4);
      if (miclen != 4) {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Invalid MIC length %d" CR), miclen);
        return;
      }

      int ret = mbedtls_ccm_auth_decrypt(
          &ctx, // AES Key
          ciphertextlen, // length of ciphertext
          nonce, noncelength, // Nonce
          aad, aadLength, // AAD
          ciphertext, // input ciphertext
          decrypted, // output plaintext
          mic, sizeof(mic) // Message Integrity Check
      );
      mbedtls_ccm_free(&ctx);

      if (ret == 0) {
        THEENGS_LOG_NOTICE(F("[BLEDecryptor] Decryption successful" CR));
      } else if (ret == MBEDTLS_ERR_CCM_AUTH_FAILED) {
        if (ble_aes_keys.containsKey(macWOdots)) {
          THEENGS_LOG_ERROR(F("[BLEDecryptor] Decryption failed for %s with key %s" CR), macWOdots.c_str(), ble_aes_keys[macWOdots].as<const char*>());
        } else {
          THEENGS_LOG_ERROR(F("[BLEDecryptor] Decryption failed for %s with default key" CR), macWOdots.c_str());
        }
        return;
      } else {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Decryption failed with error: %X" CR), ret);
        return;
      }

      // Build new servicedata
      if (BLEdata["encr"].as<int>() == 1) { // PVVX
        BLEdata["servicedata"] = NimBLEUtils::dataToHexString(decrypted, ciphertextlen);
      } else if (BLEdata["encr"].as<int>() == 2) { // BTHomeV2
        // Build new servicedata
        uint8_t newservicedata[3 + ciphertextlen];
        newservicedata[0] = 0x40; // Decrypted BTHomeV2 Packet Type
        newservicedata[1] = 0x00; // Packet counter which the PVVX BTHome non-encrypted has but the encrypted does not
        newservicedata[2] = 0x00; // **TODO Convert the ctr to the packet counter or just stick with 0?
        memcpy(&newservicedata[3], decrypted, ciphertextlen);
        BLEdata["servicedata"] = NimBLEUtils::dataToHexString(newservicedata, ciphertextlen + 3);
      } else {
        return;
      }
      THEENGS_LOG_TRACE(F("[BLEDecryptor] Decrypted servicedata %s" CR), BLEdata["servicedata"].as<const char*>());

    } else if (BLEdata["encr"].as<int>() == 3) {
      // Decrypt Victron Energy encrypted advertisements.
      size_t nc_off = 0;
      uint8_t stream_block[16] = {0};

      mbedtls_aes_context ctx;
      mbedtls_aes_init(&ctx);
      mbedtls_aes_setkey_enc(&ctx, bleaeskey, 128);
      int ret = mbedtls_aes_crypt_ctr(
          &ctx, // AES Key
          ciphertextlen, // length of ciphertext
          &nc_off,
          nonce, // 16 byte nonce with 2 bytes iv
          stream_block,
          ciphertext, // input ciphertext
          decrypted // output plaintext
      );
      mbedtls_aes_free(&ctx);

      if (ret == 0) {
        THEENGS_LOG_NOTICE(F("[BLEDecryptor] Victron Decryption successful" CR));
      } else if (ret == MBEDTLS_ERR_CCM_AUTH_FAILED) {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Victron Authentication failed." CR));
        return;
      } else {
        THEENGS_LOG_ERROR(F("[BLEDecryptor] Victron decryption failed with error: %X" CR), ret);
        return;
      }

      // Build new manufacturerdata
      unsigned char manufacturerdata[10 + ciphertextlen];
      int manufacturerdatalen = hexToBytes(BLEdata["manufacturerdata"].as<String>(), manufacturerdata, 10);
      manufacturerdata[2] = 0x11; // Replace byte 2 with "11" indicate decrypted data
      manufacturerdata[7] = 0xff; // Replace byte 7 with "ff" to indicate decrypted data
      manufacturerdata[8] = 0xff; // Replace byte 8 with "ff" to indicate decrypted data
      memcpy(&manufacturerdata[10], decrypted, ciphertextlen); // Append the decrypted payload to the manufacturer data
      BLEdata["manufacturerdata"] = NimBLEUtils::dataToHexString(manufacturerdata, 10 + ciphertextlen); // Rebuild manufacturerdata
      THEENGS_LOG_TRACE(F("[BLEDecryptor] Victron decrypted manufacturerdata %s" CR), BLEdata["manufacturerdata"].as<const char*>());
    }

    // Print before and after decoder post decryption
    // serializeJsonPretty(BLEdata, Serial);
    model_id = BTConfig.extDecoderEnable ? -1 : decoder.decodeBLEJson(BLEdata);
    // serializeJsonPretty(BLEdata, Serial);
    THEENGS_LOG_TRACE(F("[BLEDecryptor] Decrypted model_id %d" CR), model_id);

    // Remove the cipher fields from BLEdata
    BLEdata.remove("encr");
    BLEdata.remove("cipher");
    BLEdata.remove("ctr");
    BLEdata.remove("mic");
  }
#    endif

  // Convert prmacs to RMACS until or if OMG gets Identity MAC/IRK decoding
  if (BLEdata["prmac"]) {
    BLEdata.remove("prmac");
    if (BLEdata["track"]) {
      BLEdata.remove("track");
    }
    BLEdata["type"] = "RMAC";
    THEENGS_LOG_TRACE(F("Potential RMAC (prmac) converted to RMAC" CR));
  }
  const char* deviceName = BLEdata["name"] | "";

  if ((BLEdata["type"].as<string>()).compare("RMAC") != 0 && model_id != TheengsDecoder::BLE_ID_NUM::IBEACON) { // Do not store in memory the random mac devices and iBeacons
    if (model_id >= 0) { // Broadcaster devices
      THEENGS_LOG_TRACE(F("Decoder found device: %s" CR), BLEdata["model_id"].as<const char*>());
      if (model_id == TheengsDecoder::BLE_ID_NUM::HHCCJCY01HHCC || model_id == TheengsDecoder::BLE_ID_NUM::BM2 || model_id == TheengsDecoder::BLE_ID_NUM::BM6) { // Device that broadcast and can be connected
        createOrUpdateDevice(mac, device_flags_connect, model_id, mac_type, deviceName);
      } else {
        createOrUpdateDevice(mac, device_flags_init, model_id, mac_type, deviceName);
        if (BTConfig.adaptiveScan == true && (BTConfig.BLEinterval != MinTimeBtwScan || BTConfig.intervalActiveScan != MinTimeBtwScan)) {
          if (BLEdata.containsKey("acts") && BLEdata.containsKey("cont")) {
            if (BLEdata["acts"] && BLEdata["cont"]) {
              BTConfig.BLEinterval = MinTimeBtwScan;
              BTConfig.intervalActiveScan = MinTimeBtwScan;
              BTConfig.scanDuration = MinScanDuration;
              THEENGS_LOG_NOTICE(F("Active and continuous scanning required, parameters adapted" CR));
              stateBTMeasures(false);
            }
          } else if (BLEdata.containsKey("cont") && BTConfig.BLEinterval != MinTimeBtwScan) {
            if (BLEdata["cont"]) {
              BTConfig.BLEinterval = MinTimeBtwScan;
              if ((BLEdata["type"].as<string>()).compare("CTMO") == 0) {
                BTConfig.scanDuration = MinScanDuration;
              }
              THEENGS_LOG_NOTICE(F("Passive continuous scanning required, parameters adapted" CR));
              stateBTMeasures(false);
            }
          }
        }
      }
    } else {
      if (BLEdata.containsKey("name")) { // Connectable only devices
        std::string name = BLEdata["name"];
        if (name.compare("LYWSD03MMC") == 0)
          model_id = BLEconectable::id::LYWSD03MMC;
        else if (name.compare("DT24-BLE") == 0)
          model_id = BLEconectable::id::DT24_BLE;
        else if (name.compare("MHO-C401") == 0)
          model_id = BLEconectable::id::MHO_C401;
        else if (name.compare("XMWSDJ04MMC") == 0)
          model_id = BLEconectable::id::XMWSDJ04MMC;

        if (model_id > 0) {
          THEENGS_LOG_TRACE(F("Connectable device found: %s" CR), name.c_str());
          createOrUpdateDevice(mac, device_flags_connect, model_id, mac_type, deviceName);
        }
      } else if (BTConfig.extDecoderEnable && model_id < 0 && BLEdata.containsKey("servicedata")) {
        const char* service_data = (const char*)(BLEdata["servicedata"] | "");
        if (strstr(service_data, "209800") != NULL) {
          model_id = TheengsDecoder::BLE_ID_NUM::HHCCJCY01HHCC;
          THEENGS_LOG_TRACE(F("Connectable device found: HHCCJCY01HHCC" CR));
          createOrUpdateDevice(mac, device_flags_connect, model_id, mac_type, deviceName);
        }
      }
    }
  } else {
    THEENGS_LOG_TRACE(F("Random MAC or iBeacon device filtered" CR));
  }
  if (!BTConfig.extDecoderEnable && model_id < 0) {
    THEENGS_LOG_TRACE(F("No eligible device found " CR));
  }
}
void PublishDeviceData(JsonObject& BLEdata) {
  if (abs((int)BLEdata["rssi"] | 0) < abs(BTConfig.minRssi)) { // process only the devices close enough
    // Decode the payload
    process_bledata(BLEdata);
    // If the device is a random MAC and pubRandomMACs is false we don't publish this payload
    if (!BTConfig.pubRandomMACs && (BLEdata["type"].as<string>()).compare("RMAC") == 0) {
      THEENGS_LOG_TRACE(F("Random MAC, device filtered" CR));
      return;
    }
    // If pubAdvData is false we don't publish the adv data
    if (!BTConfig.pubAdvData) {
      BLEdata.remove("servicedatauuid");
      BLEdata.remove("servicedata");
      BLEdata.remove("manufacturerdata");
      BLEdata.remove("mac_type");
      BLEdata.remove("adv_type");
      // tag device properties
      // BLEdata.remove("type");   type is used by the WebUI module to determine the template used to display the signal
      BLEdata.remove("cidc");
      BLEdata.remove("acts");
      BLEdata.remove("cont");
      BLEdata.remove("track");
      BLEdata.remove("ctrl");
    }
    // if distance available, check if presenceUseBeaconUuid is true, model_id is IBEACON then set id as uuid
    if (BLEdata.containsKey("distance")) {
      if (BTConfig.presenceUseBeaconUuid && BLEdata.containsKey("model_id") && BLEdata["model_id"].as<String>() == "IBEACON") {
        BLEdata["mac"] = BLEdata["id"].as<std::string>();
        BLEdata["id"] = BLEdata["uuid"].as<std::string>();
      }
      String topic = String(mqtt_topic) + BTConfig.presenceTopic + String(gateway_name);
      THEENGS_LOG_TRACE(F("Pub HA Presence %s" CR), topic.c_str());
      BLEdata["topic"] = topic;
      enqueueJsonObject(BLEdata, QueueSemaphoreTimeOutTask);
    }

    // If the device is not a sensor and pubOnlySensors is true we don't publish this payload
    if (!BTConfig.pubOnlySensors || BLEdata.containsKey("model") || !BLEDecoder) { // Identified device
      buildTopicFromId(BLEdata, subjectBTtoMQTT);
      enqueueJsonObject(BLEdata, QueueSemaphoreTimeOutTask);
    } else {
      THEENGS_LOG_NOTICE(F("Not a sensor device filtered" CR));
      return;
    }

#    if BLEDecoder
    if (enableMultiGTWSync && BLEdata.containsKey("model_id") && BLEdata.containsKey("id")) {
      // Publish tracker sync message
      bool isTracker = false;
      std::string tag = decoder.getTheengAttribute(BLEdata["model_id"].as<const char*>(), "tag");
      if (tag.length() >= 4) {
        isTracker = checkIfIsTracker(tag[3]);
      }

      if (isTracker) {
        StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
        JsonObject TrackerSyncdata = BLEdataBuffer.to<JsonObject>();
        TrackerSyncdata["gatewayid"] = gateway_name;
        TrackerSyncdata["trackerid"] = BLEdata["id"].as<const char*>();
        String topic = String(mqtt_topic) + String(subjectTrackerSync);
        TrackerSyncdata["topic"] = topic.c_str();
        enqueueJsonObject(TrackerSyncdata);
      }
    }
#    endif
  } else {
    THEENGS_LOG_NOTICE(F("Low rssi, device filtered" CR));
    return;
  }
}
#  else
void process_bledata(JsonObject& BLEdata) {}
void PublishDeviceData(JsonObject& BLEdata) {
  if (abs((int)BLEdata["rssi"] | 0) < abs(BTConfig.minRssi)) { // process only the devices close enough
    // if distance available, check if presenceUseBeaconUuid is true, model_id is IBEACON then set id as uuid
    if (BLEdata.containsKey("distance")) {
      if (BTConfig.presenceUseBeaconUuid && BLEdata.containsKey("model_id") && BLEdata["model_id"].as<String>() == "IBEACON") {
        BLEdata["mac"] = BLEdata["id"].as<std::string>();
        BLEdata["id"] = BLEdata["uuid"].as<std::string>();
      }
      enqueueJsonObject(BLEdata, QueueSemaphoreTimeOutTask);
    }
    buildTopicFromId(BLEdata, subjectBTtoMQTT);
    enqueueJsonObject(BLEdata, QueueSemaphoreTimeOutTask);
  } else {
    THEENGS_LOG_NOTICE(F("Low rssi, device filtered" CR));
    return;
  }
}
#  endif

void hass_presence(JsonObject& HomePresence) {
  int BLErssi = HomePresence["rssi"];
  THEENGS_LOG_TRACE(F("BLErssi %d" CR), BLErssi);
  int txPower = HomePresence["txpower"] | 0;
  if (txPower >= 0)
    txPower = -59; //if tx power is not found we set a default calibration value
  THEENGS_LOG_TRACE(F("TxPower: %d" CR), txPower);
  double ratio = BLErssi * 1.0 / txPower;
  double distance;
  if (ratio < 1.0) {
    distance = pow(ratio, 10);
  } else {
    distance = (0.89976) * pow(ratio, 7.7095) + 0.111;
  }
  HomePresence["distance"] = distance;
  THEENGS_LOG_TRACE(F("Ble distance %D" CR), distance);
}

void BTforceScan() {
  if (!BTProcessLock) {
    BLEscan();
    THEENGS_LOG_TRACE(F("Scan done" CR));
    if (BTConfig.bleConnect)
      BLEconnect();
  } else {
    THEENGS_LOG_TRACE(F("Cannot launch scan due to other process running" CR));
  }
}

void immediateBTAction(void* pvParameters) {
  if (BLEactions.size()) {
    // Immediate action; we need to prevent the normal connection action and stop scanning
    BTProcessLock = true;
    NimBLEScan* pScan = NimBLEDevice::getScan();
    if (pScan->isScanning()) {
      pScan->stop();
    }

    if (xSemaphoreTake(semaphoreBLEOperation, pdMS_TO_TICKS(5000)) == pdTRUE) {
      if (xSemaphoreTake(semaphoreCreateOrUpdateDevice, pdMS_TO_TICKS(QueueSemaphoreTimeOutTask)) == pdTRUE) {
        // swap the vectors so only this device is processed
        std::vector<BLEdevice*> dev_swap;
        dev_swap.push_back(getDeviceByMac(BLEactions.back().addr.toString().c_str()));
        std::swap(devices, dev_swap);

        std::vector<BLEAction> act_swap;
        act_swap.push_back(BLEactions.back());
        BLEactions.pop_back();
        std::swap(BLEactions, act_swap);

        // Unlock here to allow the action to be performed
        BTProcessLock = false;
        BLEconnect();
        // back to normal
        std::swap(devices, dev_swap);
        std::swap(BLEactions, act_swap);
        xSemaphoreGive(semaphoreCreateOrUpdateDevice);
      } else {
        THEENGS_LOG_ERROR(F("CreateOrUpdate Semaphore NOT taken" CR));
      }

      // If we stopped the scheduled connect for this action, do the scheduled now
      if (millis() > (timeBetweenConnect + BTConfig.intervalConnect) && BTConfig.bleConnect) {
        timeBetweenConnect = millis();
        BLEconnect();
      }
      xSemaphoreGive(semaphoreBLEOperation);
    } else {
      THEENGS_LOG_ERROR(F("BLE busy - immediateBTAction not sent" CR));
      gatewayState = GatewayState::ERROR;
      StaticJsonDocument<JSON_MSG_BUFFER> BLEdataBuffer;
      JsonObject BLEdata = BLEdataBuffer.to<JsonObject>();
      BLEdata["id"] = BLEactions.back().addr.toString();
      BLEdata["success"] = false;
      buildTopicFromId(BLEdata, subjectBTtoMQTT);
      enqueueJsonObject(BLEdata, QueueSemaphoreTimeOutTask);
      BLEactions.pop_back();
      BTProcessLock = false;
    }
  }
  vTaskDelete(NULL);
}

void startBTActionTask() {
  TaskHandle_t th;
  xTaskCreateUniversal(
      immediateBTAction, /* Function to implement the task */
      "imActTask", /* Name of the task */
      8000, /* Stack size in bytes */
      NULL, /* Task input parameter */
      3, /* Priority of the task (set higher than core task) */
      &th, /* Task handle. */
      1); /* Core where the task should run */
}

#  if BLEDecoder
void KnownBTActions(JsonObject& BTdata) {
  if (!BTdata.containsKey("id")) {
    THEENGS_LOG_ERROR(F("BLE mac address missing" CR));
    gatewayState = GatewayState::ERROR;
    return;
  }

  BLEAction action{};
  action.write = true;
  action.ttl = 3;
  bool res = false;
  if (BTdata.containsKey("model_id") && BTdata["model_id"].is<const char*>()) {
    if (BTdata["model_id"] == "X1") {
      if (BTdata.containsKey("cmd") && BTdata["cmd"].is<const char*>()) {
        action.value_type = BLE_VAL_STRING;
        std::string val = BTdata["cmd"].as<std::string>(); // Fix #1694
        action.value = val;
        createOrUpdateDevice(BTdata["id"].as<const char*>(), device_flags_connect,
                             TheengsDecoder::BLE_ID_NUM::SBS1, 1);
        res = true;
      }
    } else if (BTdata["model_id"] == "W270160X") {
      if (BTdata.containsKey("tilt") && BTdata["tilt"].is<int>()) {
        action.value_type = BLE_VAL_INT;
        res = true;
      } else if (BTdata.containsKey("tilt") && BTdata["tilt"].is<const char*>()) {
        action.value_type = BLE_VAL_STRING;
        res = true;
      }
      if (res) {
        std::string val = BTdata["tilt"].as<std::string>(); // Fix #1694
        action.value = val;
        createOrUpdateDevice(BTdata["id"].as<const char*>(), device_flags_connect,
                             TheengsDecoder::BLE_ID_NUM::SBBT, 1);
      }
    } else if (BTdata["model_id"] == "W070160X") {
      if (BTdata.containsKey("position") && BTdata["position"].is<int>()) {
        action.value_type = BLE_VAL_INT;
        res = true;
      } else if (BTdata.containsKey("position") && BTdata["position"].is<const char*>()) {
        action.value_type = BLE_VAL_STRING;
        res = true;
      }
      if (res) {
        std::string val = BTdata["position"].as<std::string>(); // Fix #1694
        action.value = val;
        createOrUpdateDevice(BTdata["id"].as<const char*>(), device_flags_connect,
                             TheengsDecoder::BLE_ID_NUM::SBCU, 1);
      }
    }
    if (res) {
      action.addr = NimBLEAddress(BTdata["id"].as<std::string>(), 1);
      BLEactions.push_back(action);
      startBTActionTask();
    } else {
      THEENGS_LOG_ERROR(F("BLE action not recognized" CR));
      gatewayState = GatewayState::ERROR;
    }
  }
}
#  else
void KnownBTActions(JsonObject& BTdata) {}
#  endif

void XtoBTAction(JsonObject& BTdata) {
  BLEAction action{};
  action.ttl = BTdata.containsKey("ttl") ? (uint8_t)BTdata["ttl"] : 1;
  action.value_type = BLE_VAL_STRING;
  if (BTdata.containsKey("value_type")) {
    String vt = BTdata["value_type"];
    vt.toUpperCase();
    if (vt == "HEX")
      action.value_type = BLE_VAL_HEX;
    else if (vt == "INT")
      action.value_type = BLE_VAL_INT;
    else if (vt == "FLOAT")
      action.value_type = BLE_VAL_FLOAT;
    else if (vt != "STRING") {
      THEENGS_LOG_ERROR(F("BLE value type invalid %s" CR), vt.c_str());
      return;
    }
  }

  THEENGS_LOG_TRACE(F("BLE ACTION TTL = %u" CR), action.ttl);
  action.complete = false;
  if (BTdata.containsKey("ble_write_address") &&
      BTdata.containsKey("ble_write_service") &&
      BTdata.containsKey("ble_write_char") &&
      BTdata.containsKey("ble_write_value")) {
    action.addr = NimBLEAddress(BTdata["ble_write_address"].as<std::string>(), BTdata.containsKey("mac_type") ? BTdata["mac_type"].as<int>() : 0);
    action.service = NimBLEUUID((const char*)BTdata["ble_write_service"]);
    action.characteristic = NimBLEUUID((const char*)BTdata["ble_write_char"]);
    std::string val = BTdata["ble_write_value"].as<std::string>(); // Fix #1694
    action.value = val;
    action.write = true;
    THEENGS_LOG_TRACE(F("BLE ACTION Write" CR));
  } else if (BTdata.containsKey("ble_read_address") &&
             BTdata.containsKey("ble_read_service") &&
             BTdata.containsKey("ble_read_char")) {
    action.addr = NimBLEAddress(BTdata["ble_read_address"].as<std::string>(), BTdata.containsKey("mac_type") ? BTdata["mac_type"].as<int>() : 0);
    action.service = NimBLEUUID((const char*)BTdata["ble_read_service"]);
    action.characteristic = NimBLEUUID((const char*)BTdata["ble_read_char"]);
    action.write = false;
    THEENGS_LOG_TRACE(F("BLE ACTION Read" CR));
  } else {
    return;
  }

  createOrUpdateDevice(action.addr.toString().c_str(), device_flags_connect, UNKWNON_MODEL, action.addr.getType());

  BLEactions.push_back(action);
  if (BTdata.containsKey("immediate") && BTdata["immediate"].as<bool>()) {
    startBTActionTask();
  }
}

void XtoBT(const char* topicOri, JsonObject& BTdata) { // json object decoding
  if (cmpToMainTopic(topicOri, subjectMQTTtoBTset)) {
    THEENGS_LOG_TRACE(F("MQTTtoBT json set" CR));

    // Black list & white list set
    bool WorBupdated;
    WorBupdated = updateWorB(BTdata, true);
    WorBupdated |= updateWorB(BTdata, false);

    if (WorBupdated) {
      if (xSemaphoreTake(semaphoreCreateOrUpdateDevice, pdMS_TO_TICKS(QueueSemaphoreTimeOutTask)) == pdTRUE) {
        //dumpDevices();
        xSemaphoreGive(semaphoreCreateOrUpdateDevice);
      }
    }

    // Force scan now
    if (BTdata.containsKey("interval") && BTdata["interval"] == 0) {
      THEENGS_LOG_NOTICE(F("BLE forced scan" CR));
      atomic_store_explicit(&forceBTScan, 1, ::memory_order_seq_cst); // ask the other core to do the scan for us
    }

    /*
     * Configuration modifications priorities:
     *  First `init=true` and `load=true` commands are executed (if both are present, INIT prevails on LOAD)
     *  Then parameters included in json are taken in account
     *  Finally `erase=true` and `save=true` commands are executed (if both are present, ERASE prevails on SAVE)
     */
    if (BTdata.containsKey("init") && BTdata["init"].as<bool>()) {
      // Restore the default (initial) configuration
      BTConfig_init();
    } else if (BTdata.containsKey("load") && BTdata["load"].as<bool>()) {
      // Load the saved configuration, if not initialised
      BTConfig_load();
    }

    // Load config from json if available
    BTConfig_fromJson(BTdata);

  } else if (cmpToMainTopic(topicOri, subjectMQTTtoBT)) {
    if (xSemaphoreTake(semaphoreBLEOperation, pdMS_TO_TICKS(5000)) == pdTRUE) {
      KnownBTActions(BTdata);
      XtoBTAction(BTdata);
      xSemaphoreGive(semaphoreBLEOperation);
    } else {
      THEENGS_LOG_ERROR(F("BLE busy - BTActions not sent" CR));
      gatewayState = GatewayState::ERROR;
    }
  } else if (strstr(topicOri, subjectTrackerSync) != NULL) {
    if (BTdata.containsKey("gatewayid") && BTdata.containsKey("trackerid") && BTdata["gatewayid"] != gateway_name) {
      BLEdevice* device = getDeviceByMac(BTdata["trackerid"].as<const char*>());
      if (device != &NO_BT_DEVICE_FOUND && device->lastUpdate != 0) {
        device->lastUpdate = 0;
        THEENGS_LOG_NOTICE(F("Tracker %s disassociated by gateway %s" CR), BTdata["trackerid"].as<const char*>(), BTdata["gatewayid"].as<const char*>());
      }
    }
  }
}
#endif