diff --git a/BLE.ino b/BLE.ino
deleted file mode 100644
index 3335370..0000000
--- a/BLE.ino
+++ /dev/null
@@ -1,347 +0,0 @@
-BLEScan* pBLEScan = nullptr;
-BLEClient* pClient = nullptr;
-BLEAdvertisedDevice* pRemoteDevice = nullptr;
-BLERemoteService* pRemoteService = nullptr;
-BLERemoteCharacteristic* pRemoteCharacteristic_rx = nullptr;
-BLERemoteCharacteristic* pRemoteCharacteristic_tx = nullptr;
-
-// 0000ff01-0000-1000-8000-00805f9b34fb
-// Notifications from this characteristic is received data from BMS
-// NOTIFY, READ
-
-// 0000ff02-0000-1000-8000-00805f9b34fb
-// Write this characteristic to send data to BMS
-// READ, WRITE, WRITE NO RESPONSE
-
-boolean doScan = false; // becomes true when BLE is initialized and scanning is allowed
-boolean doConnect = false; // becomes true when correct ID is found during scanning
-
-boolean ble_client_connected = false; // true when fully connected
-
-unsigned int ble_packets_requested = 0b00; // keeps track of requested packets
-unsigned int ble_packets_received = 0b00; // keeps track of received packets
-
-
-// ======= CALLBACKS =========
-
-void MyEndOfScanCallback(BLEScanResults pBLEScanResult){
-    bms_status=false; // BMS not found
-
-    if(BLE_CALLBACK_DEBUG){
-      debug("BLE: scan finished");
-      Serial.println("Scan finished.");
-    }
-}
-
-class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks{
-  // called for each advertising BLE server
-
-  void onResult(BLEAdvertisedDevice advertisedDevice){
-    // found a device
-
-
-    if(BLE_CALLBACK_DEBUG){
-      debug(
-          String("BLE: found ") +
-          String(advertisedDevice.getName().c_str()) +
-          String(" with address ") +
-          String(advertisedDevice.getAddress().toString().c_str()) +
-          String(" and RSSI ") +
-          String(advertisedDevice.getRSSI())
-        );
-
-      Serial.print("BLE: found ");
-      Serial.println(advertisedDevice.toString().c_str());
-    }
-
-    // Check if device is advertising the specific service UUID
-    if (!advertisedDevice.isAdvertisingService(serviceUUID)) {
-      debug("Device does not advertise the specified service UUID.");
-      return;
-    }
-
-      if(BLE_CALLBACK_DEBUG){
-        debug("BLE: target device found");
-      }
-
-      pBLEScan->stop();
-
-        // delete old remote device, create new one
-        if(pRemoteDevice != nullptr){
-          delete pRemoteDevice;
-        }
-        pRemoteDevice = new BLEAdvertisedDevice(advertisedDevice);
-
-        doConnect = true;
-  }
-};
-
-class MyClientCallback : public BLEClientCallbacks{
-  // called on connect/disconnect
-  void onConnect(BLEClient* pclient){
-
-    if(BLE_CALLBACK_DEBUG){
-      debug(String("BLE: connecting to ") + String(pclient->getPeerAddress().toString().c_str()));
-    }
-  }
-
-  void onDisconnect(BLEClient* pclient){
-    ble_client_connected = false;
-    doConnect = false;
-
-    if(BLE_CALLBACK_DEBUG){
-      debug(String("BLE: disconnected from ") + String(pclient->getPeerAddress().toString().c_str()));
-    }
-
-  }
-};
-
-static void MyNotifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify){
-  //this is called when BLE server sents data via notification
-  //hexDump((char*)pData, length);
-  if(!bleCollectPacket((char *)pData, length)){
-    debug("ERROR: packet could not be collected.");
-  }
-}
-
-
-
-
-
-// ======= OTHERS =========
-
-void handleBLE(){
-  static unsigned long prev_millis_standby = 0;
-
-  prev_millis_standby = millis();
-
-  while(true){ // loop until we hit a timeout or gathered all packets
-
-    if((ble_packets_received == BLE_PACKETSRECEIVED_BEFORE_STANDBY) || (millis()>prev_millis_standby+BLE_TIMEOUT)){
-      if(ble_packets_received == BLE_PACKETSRECEIVED_BEFORE_STANDBY){
-        debug("BLE: all packets received");
-        bms_status=true; // BMS was connected, data up-to-date
-        printBasicInfo();
-        printCellInfo();
-      }else{
-        debug("BLE: connection timeout");
-        bms_status=false; // BMS not (fully) connected
-      }
-
-      break; // we're done with BLE, exit while loop
-    }
-    else if (doConnect){
-
-      // found the desired BLE server, now connect to it
-      if (connectToServer()){
-        ble_client_connected = true;
-        ble_packets_received=0;
-        ble_packets_requested=0;
-
-      }else{
-        ble_client_connected = false;
-        debug("BLE: failed to connect");
-      }
-
-      doConnect = false;
-    }
-
-    if (ble_client_connected){
-      // if connected to BLE server, request all data
-      if((ble_packets_requested & 0b01)!=0b01){
-        // request packet 0b01
-        debug("BLE: requesting packet 0b01");
-        delay(50);
-        if(bmsRequestBasicInfo()){
-          ble_packets_requested |= 0b01;
-        }
-
-      }else if(((ble_packets_received & 0b01)==0b01) && ((ble_packets_requested & 0b10)!=0b10)){
-        // request packet 0b10 after 0b01 has been received
-        debug("BLE: requesting packet 0b10");
-        delay(50);
-        if(bmsRequestCellInfo()){
-          ble_packets_requested |= 0b10;
-        }
-      }
-
-    }else if ((!doConnect)&&(doScan)){
-      // we are not connected, so we can scan for devices
-      debug("BLE: not connected, starting scan");
-      Serial.print("BLE is not connected, starting scan");
-
-      // Disconnect client
-      if((pClient != nullptr)&&(pClient->isConnected())){
-        pClient->disconnect();
-      }
-
-      // stop scan (if running) and start a new one
-      pBLEScan->setActiveScan(true);
-      pBLEScan->setInterval(1 << 8); // 160 ms
-      pBLEScan->setWindow(1 << 7); // 80 ms
-      pBLEScan->start(BLE_SCAN_DURATION, MyEndOfScanCallback, false); // non-blocking, use a callback
-
-      doScan=false;
-
-      debug("BLE: scan started");
-    }
-  }
-}
-
-void bleGatherPackets(){
-  bleStart();
-  handleBLE();
-  blePause();
-  BLEDevice::deinit(false);
-}
-
-void bleStart(){
-  Serial.print("Starting BLE... ");
-
-  BLEDevice::init("");
-  //esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT); // release some unused memory
-
-  // Retrieve a BLE client
-  pClient = BLEDevice::createClient();
-  pClient->setClientCallbacks(new MyClientCallback());
-
-  // Retrieve a BLE scanner
-  pBLEScan = BLEDevice::getScan();
-  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
-
-  bleContinue();
-  Serial.println("done");
-}
-
-void blePause(){
-  // stop scanning and disconnect from all devices
-  doScan=false;
-
-  // Disconnect client
-  if((pClient != nullptr)&&(pClient->isConnected())){
-    pClient->disconnect();
-  }
-
-  delay(50);
-
-  pBLEScan->stop();
-
-  ble_client_connected=false;
-  doConnect=false;
-  ble_packets_received=0;
-  ble_packets_requested=0;
-
-}
-
-
-void bleContinue(){
-  // Prepare for scanning
-  ble_client_connected=false;
-  doConnect=false;
-  ble_packets_received=0;
-
-  doScan=true; // start scanning for new devices
-}
-
-bool connectToServer(){
-  if(pRemoteDevice==nullptr){
-    Serial.println("Invalid remote device, can't connect");
-    return false;
-  }
-
-  // Disconnect client
-  if((pClient != nullptr)&&(pClient->isConnected())){
-    pClient->disconnect();
-  }
-
-  Serial.print("Forming a connection to ");
-  Serial.println(pRemoteDevice->getAddress().toString().c_str());
-
-  delay(100);
-
-  // Connect to the remote BLE Server.
-  pClient->connect(pRemoteDevice);
-  if(!(pClient->isConnected())){
-    debug(String("BLE: failed to connect"));
-    Serial.println("Failed to connect to server");
-    pClient->disconnect();
-    return false;
-  }
-
-  Serial.println(" - Connected to server");
-
-
-  // Get remote service
-  pRemoteService = pClient->getService(serviceUUID);
-  if (pRemoteService == nullptr){
-    debug(String("BLE: failed to find service UUID"));
-    Serial.print("Failed to find our service UUID: ");
-    Serial.println(serviceUUID.toString().c_str());
-    pClient->disconnect();
-    return false;
-  }
-  Serial.println(" - Found our service");
-
-
-  // Get BMS receive characteristic
-  pRemoteCharacteristic_rx = pRemoteService->getCharacteristic(charUUID_rx);
-  if (pRemoteCharacteristic_rx == nullptr){
-    debug(String("BLE: failed to find RX UUID"));
-    Serial.print("Failed to find rx UUID: ");
-    Serial.println(charUUID_rx.toString().c_str());
-    pClient->disconnect();
-    return false;
-  }
-  Serial.println(" - Found RX characteristic");
-
-
-  // Register callback for remote characteristic (receive channel)
-  if (pRemoteCharacteristic_rx->canNotify()){
-      pRemoteCharacteristic_rx->registerForNotify(MyNotifyCallback);
-  }else{
-    debug(String("BLE: failed to register notification of remote characteristic"));
-    Serial.println("Failed to register notification of remote characteristic");
-    pClient->disconnect();
-    return false;
-  }
-  Serial.println(" - Registered remote characteristic for notification");
-
-
-  // Get BMS transmit characteristic
-  pRemoteCharacteristic_tx = pRemoteService->getCharacteristic(charUUID_tx);
-  if (pRemoteCharacteristic_tx == nullptr){
-    debug(String("BLE: failed to find TX UUID"));
-    Serial.print("Failed to find tx UUID: ");
-    Serial.println(charUUID_tx.toString().c_str());
-    pClient->disconnect();
-    return false;
-  }
-  Serial.println(" - Found TX characteristic");
-
-
-  // Check whether tx is writeable
-  if (!(pRemoteCharacteristic_tx->canWriteNoResponse())){
-    debug(String("BLE: failed TX remote characteristic is not writable"));
-    Serial.println("Failed TX remote characteristic is not writable");
-    pClient->disconnect();
-    return false;
-  }
-  Serial.println(" - TX is writable");
-
-
-  delay(BLE_REQUEST_DELAY); // wait, otherwise writeValue doesn't work for some reason
-                            // to do: fix this ugly hack
-
-  debug(String("BLE: connected"));
-
-  return true;
-}
-
-bool sendCommand(uint8_t *data, uint32_t dataLen){
-  if((pClient!=nullptr)&&(pClient->isConnected())){
-    pRemoteCharacteristic_tx->writeValue(data, dataLen, false);
-    return true;
-  }else{
-    return false;
-  }
-}
diff --git a/BMS_process_data.ino b/BMS_process_data.ino
deleted file mode 100644
index 11757ad..0000000
--- a/BMS_process_data.ino
+++ /dev/null
@@ -1,265 +0,0 @@
-bool isPacketValid(byte *packet) //check if packet is valid
-{
-    if (packet == nullptr){
-        return false;
-    }
-
-    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
-    int checksumPos = pHeader->dataLen + 2; // status + data len + data
-
-    int offset = 2; // header 0xDD and command type are not in data length
-
-    if (packet[0] != 0xDD){
-        // start bit missing
-        return false;
-    }
-
-    if (packet[offset + checksumPos + 2] != 0x77){
-        // stop bit missing
-        return false;
-    }
-
-    byte checksum = 0;
-    for (int i = 0; i < checksumPos; i++){
-        checksum += packet[offset + i];
-    }
-    checksum = ((checksum ^ 0xFF) + 1) & 0xFF;
-
-    if (checksum != packet[offset + checksumPos + 1]){
-        return false;
-    }
-
-    return true;
-}
-
-bool processBasicInfo(packBasicInfoStruct *output, byte *data, unsigned int dataLen)
-{
-    // Expected data len
-    if (dataLen != 0x1B)
-    {
-        return false;
-    }
-
-    output->Volts = ((uint32_t)two_ints_into16(data[0], data[1])) * 10; // Resolution 10 mV -> convert to milivolts   eg 4895 > 48950mV
-    output->Amps = ((int32_t)two_ints_into16(data[2], data[3])) * 10;   // Resolution 10 mA -> convert to miliamps
-
-    output->Watts = output->Volts * output->Amps / 1000000; // W
-
-    output->CapacityRemainAh = ((uint16_t)two_ints_into16(data[4], data[5])) * 10;
-    output->CapacityRemainPercent = ((uint8_t)data[19]);
-
-    output->Temp1 = (((uint16_t)two_ints_into16(data[23], data[24])) - 2731);
-    output->Temp2 = (((uint16_t)two_ints_into16(data[25], data[26])) - 2731);
-    output->BalanceCodeLow = (two_ints_into16(data[12], data[13]));
-    output->BalanceCodeHigh = (two_ints_into16(data[14], data[15]));
-    output->MosfetStatus = ((byte)data[20]);
-
-    return true;
-}
-
-bool processCellInfo(packCellInfoStruct *output, byte *data, unsigned int dataLen)
-{
-    uint16_t _cellSum;
-    uint16_t _cellMin = 5000;
-    uint16_t _cellMax = 0;
-    uint16_t _cellAvg;
-    uint16_t _cellDiff;
-
-    output->NumOfCells = dataLen / 2; // data contains 2 bytes per cell
-
-    //go trough individual cells
-    for (byte i = 0; i < dataLen / 2; i++)
-    {
-        output->CellVolt[i] = ((uint16_t)two_ints_into16(data[i * 2], data[i * 2 + 1])); // Resolution 1 mV
-        _cellSum += output->CellVolt[i];
-        if (output->CellVolt[i] > _cellMax)
-        {
-            _cellMax = output->CellVolt[i];
-        }
-        if (output->CellVolt[i] < _cellMin)
-        {
-            _cellMin = output->CellVolt[i];
-        }
-    }
-
-    output->CellMin = _cellMin;
-    output->CellMax = _cellMax;
-    output->CellDiff = _cellMax - _cellMin;
-    output->CellAvg = _cellSum / output->NumOfCells;
-
-    return true;
-}
-
-bool bmsProcessPacket(byte *packet)
-{
-    bool isValid = isPacketValid(packet);
-
-    if (isValid != true)
-    {
-        Serial.println("Invalid packer received");
-        return false;
-    }
-
-    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
-    byte *data = packet + sizeof(bmsPacketHeaderStruct); // TODO Fix this ugly hack
-    unsigned int dataLen = pHeader->dataLen;
-
-    bool result = false;
-
-    // find packet type (basic info or cell info)
-    switch (pHeader->type)
-    {
-    case cBasicInfo:
-    {
-        // Process basic info
-        result = processBasicInfo(&packBasicInfo, data, dataLen);
-        if(result==true){
-          ble_packets_received |= 0b01;
-          bms_last_update_time=millis();
-        }
-
-        break;
-    }
-
-    case cCellInfo:
-    {
-        // Process cell info
-        result = processCellInfo(&packCellInfo, data, dataLen);
-        if(result==true){
-          ble_packets_received |= 0b10;
-          bms_last_update_time=millis();
-        }
-        break;
-    }
-
-    default:
-        result = false;
-        Serial.printf("Unsupported packet type detected. Type: %d", pHeader->type);
-    }
-
-    return result;
-}
-
-bool bleCollectPacket(char *data, uint32_t dataSize) // reconstruct packet, called by notifyCallback function
-{
-    static uint8_t packetstate = 0; //0 - empty, 1 - first half of packet received, 2- second half of packet received
-
-    // packet sizes:
-    //  (packet ID 03) = 4 (header) + 23 + 2*N_NTCs + 2 (checksum) + 1 (stop)
-    //  (packet ID 04) = 4 (header) + 2*NUM_CELLS   + 2 (checksum) + 1 (stop)
-    static uint8_t packetbuff[4 + 2*25 + 2 + 1] = {0x0}; // buffer size suitable for up to 25 cells
-
-    static uint32_t totalDataSize = 0;
-    bool retVal = false;
-    //hexDump(data,dataSize);
-
-    if(totalDataSize + dataSize > sizeof(packetbuff)){
-      Serial.printf("ERROR: datasize is overlength.");
-
-      debug(
-        String("ERROR: datasize is overlength. ") +
-        String("allocated=") +
-        String(sizeof(packetbuff)) +
-        String(", size=") +
-        String(totalDataSize + dataSize)
-        );
-
-      totalDataSize = 0;
-      packetstate = 0;
-
-      retVal = false;
-    }
-    else if (data[0] == 0xdd && packetstate == 0) // probably got 1st half of packet
-    {
-        packetstate = 1;
-        for (uint8_t i = 0; i < dataSize; i++)
-        {
-            packetbuff[i] = data[i];
-        }
-        totalDataSize = dataSize;
-        retVal = true;
-    }
-    else if (data[dataSize - 1] == 0x77 && packetstate == 1) //probably got 2nd half of the packet
-    {
-        packetstate = 2;
-        for (uint8_t i = 0; i < dataSize; i++)
-        {
-            packetbuff[i + totalDataSize] = data[i];
-        }
-        totalDataSize += dataSize;
-        retVal = true;
-    }
-
-    if (packetstate == 2) //got full packet
-    {
-        uint8_t packet[totalDataSize];
-        memcpy(packet, packetbuff, totalDataSize);
-
-        bmsProcessPacket(packet); //pass pointer to retrieved packet to processing function
-        packetstate = 0;
-        totalDataSize = 0;
-        retVal = true;
-    }
-    return retVal;
-}
-
-bool bmsRequestBasicInfo(){
-    // header status command length data checksum footer
-    //   DD     A5      03     00    FF     FD      77
-    uint8_t data[7] = {0xdd, 0xa5, cBasicInfo, 0x0, 0xff, 0xfd, 0x77};
-    return sendCommand(data, sizeof(data));
-}
-
-bool bmsRequestCellInfo(){
-    // header status command length data checksum footer
-    //  DD      A5      04     00    FF     FC      77
-    uint8_t data[7] = {0xdd, 0xa5, cCellInfo, 0x0, 0xff, 0xfc, 0x77};
-    return sendCommand(data, sizeof(data));
-}
-
-void printBasicInfo() //debug all data to uart
-{
-    Serial.printf("Total voltage: %f\n", (float)packBasicInfo.Volts / 1000);
-    Serial.printf("Amps: %f\n", (float)packBasicInfo.Amps / 1000);
-    Serial.printf("CapacityRemainAh: %f\n", (float)packBasicInfo.CapacityRemainAh / 1000);
-    Serial.printf("CapacityRemainPercent: %d\n", packBasicInfo.CapacityRemainPercent);
-    Serial.printf("Temp1: %f\n", (float)packBasicInfo.Temp1 / 10);
-    Serial.printf("Temp2: %f\n", (float)packBasicInfo.Temp2 / 10);
-    Serial.printf("Balance Code Low: 0x%x\n", packBasicInfo.BalanceCodeLow);
-    Serial.printf("Balance Code High: 0x%x\n", packBasicInfo.BalanceCodeHigh);
-    Serial.printf("Mosfet Status: 0x%x\n", packBasicInfo.MosfetStatus);
-}
-
-void printCellInfo() //debug all data to uart
-{
-    Serial.printf("Number of cells: %u\n", packCellInfo.NumOfCells);
-    for (byte i = 1; i <= packCellInfo.NumOfCells; i++)
-    {
-        Serial.printf("Cell no. %u", i);
-        Serial.printf("   %f\n", (float)packCellInfo.CellVolt[i - 1] / 1000);
-    }
-    Serial.printf("Max cell volt: %f\n", (float)packCellInfo.CellMax / 1000);
-    Serial.printf("Min cell volt: %f\n", (float)packCellInfo.CellMin / 1000);
-    Serial.printf("Difference cell volt: %f\n", (float)packCellInfo.CellDiff / 1000);
-    Serial.printf("Average cell volt: %f\n", (float)packCellInfo.CellAvg / 1000);
-    Serial.println();
-}
-
-void hexDump(const char *data, uint32_t dataSize) //debug function
-{
-    Serial.println("HEX data:");
-
-    for (int i = 0; i < dataSize; i++)
-    {
-        Serial.printf("0x%x, ", data[i]);
-    }
-    Serial.println("");
-}
-
-int16_t two_ints_into16(int highbyte, int lowbyte) // turns two bytes into a single long integer
-{
-    int16_t result = (highbyte);
-    result <<= 8;                //Left shift 8 bits,
-    result = (result | lowbyte); //OR operation, merge the two
-    return result;
-}
diff --git a/ESPBMS.ino b/ESPBMS.ino
index a9045f1..a2c5481 100644
--- a/ESPBMS.ino
+++ b/ESPBMS.ino
@@ -1,40 +1,3 @@
-/*
- * Xiaoxiang BMS to MQTT via WiFi
- * by Bas Vermulst - https://github.com/BeaverUI/ESP32-BluetoothBMS2MQTT
- *
- * Based on original work from https://github.com/kolins-cz/Smart-BMS-Bluetooth-ESP32/blob/master/README.md
- *
-
-   === configuring ===
-   Using the #define parameters in the CONFIGURATION section, do the following:
-   1) configure WiFi via WIFI_SSID and WIFI_PASSWORD
-   2) configure MQTT broker via MQTTSERVER
-   3) set unique node name via NODE_NAME
-   4) ensure the BMS settings are OK. You can verify the name and address using the "BLE Scanner" app on an Android phone.
-
-
-   === compiling ===
-   1) Add ESP-WROVER to the board manager:
-   - File --> Preferences, add to board manager URLs: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_index.json
-   - Then, Tools --> Board --> Board manager... --> install ESP32 package v2.0.3 or later.
-
-
-   2) Install required libraries:
-   - MQTT (by Joel Gaehwiler)
-   Install via "Manage Libraries..." under Tools.
-
-   3) Configure board:
-   Select Tools --> Board --> "ESP32 Arduino" --> "ESP WRover module"
-
-   4) Connect board via USB.
-
-   5) Configure programming options:
-   Select appropriate programming port under Tools --> Port
-   Select correct partitioning: 1.9MB app (with OTA) or 2MB app with 2 MB SPIFFS - otherwise it won't fit
-
-   6) Program and go!
-
- */
 
 // ==== CONFIGURATION ====
 // BMS
@@ -90,38 +53,593 @@ void loop(){
     bleGatherPackets();
 }
 
-/*
-  mqttclient.publish(GetTopic("ip"), IPAddressString(WiFi.localIP()));
-  mqttclient.publish(GetTopic("free-heap"), String(ESP.getFreeHeap()));
-  mqttclient.publish(GetTopic("maxalloc-heap"), String(xPortGetMinimumEverFreeHeapSize()));
-  mqttclient.publish(GetTopic("ssid"), WiFi.SSID());
-  mqttclient.publish(GetTopic("rssi"), String(WiFi.RSSI()));
+BLEScan* pBLEScan = nullptr;
+BLEClient* pClient = nullptr;
+BLEAdvertisedDevice* pRemoteDevice = nullptr;
+BLERemoteService* pRemoteService = nullptr;
+BLERemoteCharacteristic* pRemoteCharacteristic_rx = nullptr;
+BLERemoteCharacteristic* pRemoteCharacteristic_tx = nullptr;
 
-  mqttclient.publish(GetTopic("reset-reason"), String(GetResetReason(0)) + String(" | ") + String(GetResetReason(1)));
+boolean doScan = false; // becomes true when BLE is initialized and scanning is allowed
+boolean doConnect = false; // becomes true when correct ID is found during scanning
 
-  mqttclient.publish(GetTopic("runtime"), String(millis()/1000));
-  mqttclient.publish(GetTopic("battery-voltage"), String(battery_voltage,2));
+boolean ble_client_connected = false; // true when fully connected
 
-  mqttclient.publish(GetTopic("bms-status"), String(bms_status));
-  mqttclient.publish(GetTopic("bms-status-age"), String( (millis()-bms_last_update_time)/1000 ));
+unsigned int ble_packets_requested = 0b00; // keeps track of requested packets
+unsigned int ble_packets_received = 0b00; // keeps track of received packets
 
-  if(bms_status){
-    mqttclient.publish(GetTopic("number-of-cells"), String(packCellInfo.NumOfCells));
-    mqttclient.publish(GetTopic("current"), String((float)packBasicInfo.Amps / 1000,2));
-    mqttclient.publish(GetTopic("voltage"), String((float)packBasicInfo.Volts / 1000,2));
-    if(packCellInfo.NumOfCells != 0){
-      mqttclient.publish(GetTopic("cell-voltage"), String((float)packBasicInfo.Volts /(1000*packCellInfo.NumOfCells), 2));
+void MyEndOfScanCallback(BLEScanResults pBLEScanResult){
+    bms_status=false; // BMS not found
+
+    if(BLE_CALLBACK_DEBUG){
+      debug("BLE: scan finished");
+      Serial.println("Scan finished.");
     }
-    mqttclient.publish(GetTopic("cell-diff"), String((float)packCellInfo.CellDiff, 0));
-    mqttclient.publish(GetTopic("soc"), String((float)packBasicInfo.CapacityRemainPercent,1));
-
-    mqttclient.publish(GetTopic("temperature-1"), String((float)packBasicInfo.Temp1 / 10,1));
-    mqttclient.publish(GetTopic("temperature-2"), String((float)packBasicInfo.Temp2 / 10,1));
-  }
-
-  MqttPublishDebug();
-
-  mqttclient.loop();
 }
 
-*/
+class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks{
+  // called for each advertising BLE server
+
+  void onResult(BLEAdvertisedDevice advertisedDevice){
+    // found a device
+
+
+    if(BLE_CALLBACK_DEBUG){
+      debug(
+          String("BLE: found ") +
+          String(advertisedDevice.getName().c_str()) +
+          String(" with address ") +
+          String(advertisedDevice.getAddress().toString().c_str()) +
+          String(" and RSSI ") +
+          String(advertisedDevice.getRSSI())
+        );
+
+      Serial.print("BLE: found ");
+      Serial.println(advertisedDevice.toString().c_str());
+    }
+
+    // Check if device is advertising the specific service UUID
+    if (!advertisedDevice.isAdvertisingService(serviceUUID)) {
+      debug("Device does not advertise the specified service UUID.");
+      return;
+    }
+
+      if(BLE_CALLBACK_DEBUG){
+        debug("BLE: target device found");
+      }
+
+      pBLEScan->stop();
+
+        // delete old remote device, create new one
+        if(pRemoteDevice != nullptr){
+          delete pRemoteDevice;
+        }
+        pRemoteDevice = new BLEAdvertisedDevice(advertisedDevice);
+
+        doConnect = true;
+  }
+};
+
+class MyClientCallback : public BLEClientCallbacks{
+  // called on connect/disconnect
+  void onConnect(BLEClient* pclient){
+
+    if(BLE_CALLBACK_DEBUG){
+      debug(String("BLE: connecting to ") + String(pclient->getPeerAddress().toString().c_str()));
+    }
+  }
+
+  void onDisconnect(BLEClient* pclient){
+    ble_client_connected = false;
+    doConnect = false;
+
+    if(BLE_CALLBACK_DEBUG){
+      debug(String("BLE: disconnected from ") + String(pclient->getPeerAddress().toString().c_str()));
+    }
+
+  }
+};
+
+static void MyNotifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify){
+  //this is called when BLE server sents data via notification
+  //hexDump((char*)pData, length);
+  if(!bleCollectPacket((char *)pData, length)){
+    debug("ERROR: packet could not be collected.");
+  }
+}
+
+void handleBLE(){
+  static unsigned long prev_millis_standby = 0;
+
+  prev_millis_standby = millis();
+
+  while(true){ // loop until we hit a timeout or gathered all packets
+
+    if((ble_packets_received == BLE_PACKETSRECEIVED_BEFORE_STANDBY) || (millis()>prev_millis_standby+BLE_TIMEOUT)){
+      if(ble_packets_received == BLE_PACKETSRECEIVED_BEFORE_STANDBY){
+        debug("BLE: all packets received");
+        bms_status=true; // BMS was connected, data up-to-date
+        printBasicInfo();
+        printCellInfo();
+      }else{
+        debug("BLE: connection timeout");
+        bms_status=false; // BMS not (fully) connected
+      }
+
+      break; // we're done with BLE, exit while loop
+    }
+    else if (doConnect){
+
+      // found the desired BLE server, now connect to it
+      if (connectToServer()){
+        ble_client_connected = true;
+        ble_packets_received=0;
+        ble_packets_requested=0;
+
+      }else{
+        ble_client_connected = false;
+        debug("BLE: failed to connect");
+      }
+
+      doConnect = false;
+    }
+
+    if (ble_client_connected){
+        debug("BLE: requesting packet 0b01");
+        delay(5000);
+        bmsRequestBasicInfo();
+        debug("BLE: requesting packet 0b10");
+        delay(5000);
+        bmsRequestCellInfo();
+    }else if ((!doConnect)&&(doScan)){
+      // we are not connected, so we can scan for devices
+      debug("BLE: not connected, starting scan");
+      Serial.print("BLE is not connected, starting scan");
+
+      // Disconnect client
+      if((pClient != nullptr)&&(pClient->isConnected())){
+        pClient->disconnect();
+      }
+
+      // stop scan (if running) and start a new one
+      pBLEScan->setActiveScan(true);
+      pBLEScan->setInterval(1 << 8); // 160 ms
+      pBLEScan->setWindow(1 << 7); // 80 ms
+      pBLEScan->start(BLE_SCAN_DURATION, MyEndOfScanCallback, false); // non-blocking, use a callback
+
+      doScan=false;
+
+      debug("BLE: scan started");
+    }
+  }
+}
+
+void bleGatherPackets(){
+  bleStart();
+  handleBLE();
+  blePause();
+  BLEDevice::deinit(false);
+}
+
+void bleStart(){
+  Serial.print("Starting BLE... ");
+
+  BLEDevice::init("");
+  //esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT); // release some unused memory
+
+  // Retrieve a BLE client
+  pClient = BLEDevice::createClient();
+  pClient->setClientCallbacks(new MyClientCallback());
+
+  // Retrieve a BLE scanner
+  pBLEScan = BLEDevice::getScan();
+  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
+
+  bleContinue();
+  Serial.println("done");
+}
+
+void blePause(){
+  // stop scanning and disconnect from all devices
+  doScan=false;
+
+  // Disconnect client
+  if((pClient != nullptr)&&(pClient->isConnected())){
+    pClient->disconnect();
+  }
+
+  delay(50);
+
+  pBLEScan->stop();
+
+  ble_client_connected=false;
+  doConnect=false;
+  ble_packets_received=0;
+  ble_packets_requested=0;
+
+}
+
+void bleContinue(){
+  // Prepare for scanning
+  ble_client_connected=false;
+  doConnect=false;
+  ble_packets_received=0;
+
+  doScan=true; // start scanning for new devices
+}
+
+bool connectToServer(){
+  if(pRemoteDevice==nullptr){
+    Serial.println("Invalid remote device, can't connect");
+    return false;
+  }
+
+  // Disconnect client
+  if((pClient != nullptr)&&(pClient->isConnected())){
+    pClient->disconnect();
+  }
+
+  Serial.print("Forming a connection to ");
+  Serial.println(pRemoteDevice->getAddress().toString().c_str());
+
+  delay(100);
+
+  // Connect to the remote BLE Server.
+  pClient->connect(pRemoteDevice);
+  if(!(pClient->isConnected())){
+    debug(String("BLE: failed to connect"));
+    Serial.println("Failed to connect to server");
+    pClient->disconnect();
+    return false;
+  }
+
+  Serial.println(" - Connected to server");
+
+
+  // Get remote service
+  pRemoteService = pClient->getService(serviceUUID);
+  if (pRemoteService == nullptr){
+    debug(String("BLE: failed to find service UUID"));
+    Serial.print("Failed to find our service UUID: ");
+    Serial.println(serviceUUID.toString().c_str());
+    pClient->disconnect();
+    return false;
+  }
+  Serial.println(" - Found our service");
+
+
+  // Get BMS receive characteristic
+  pRemoteCharacteristic_rx = pRemoteService->getCharacteristic(charUUID_rx);
+  if (pRemoteCharacteristic_rx == nullptr){
+    debug(String("BLE: failed to find RX UUID"));
+    Serial.print("Failed to find rx UUID: ");
+    Serial.println(charUUID_rx.toString().c_str());
+    pClient->disconnect();
+    return false;
+  }
+  Serial.println(" - Found RX characteristic");
+
+
+  // Register callback for remote characteristic (receive channel)
+  if (pRemoteCharacteristic_rx->canNotify()){
+      pRemoteCharacteristic_rx->registerForNotify(MyNotifyCallback);
+  }else{
+    debug(String("BLE: failed to register notification of remote characteristic"));
+    Serial.println("Failed to register notification of remote characteristic");
+    pClient->disconnect();
+    return false;
+  }
+  Serial.println(" - Registered remote characteristic for notification");
+
+
+  // Get BMS transmit characteristic
+  pRemoteCharacteristic_tx = pRemoteService->getCharacteristic(charUUID_tx);
+  if (pRemoteCharacteristic_tx == nullptr){
+    debug(String("BLE: failed to find TX UUID"));
+    Serial.print("Failed to find tx UUID: ");
+    Serial.println(charUUID_tx.toString().c_str());
+    pClient->disconnect();
+    return false;
+  }
+  Serial.println(" - Found TX characteristic");
+
+
+  // Check whether tx is writeable
+  if (!(pRemoteCharacteristic_tx->canWriteNoResponse())){
+    debug(String("BLE: failed TX remote characteristic is not writable"));
+    Serial.println("Failed TX remote characteristic is not writable");
+    pClient->disconnect();
+    return false;
+  }
+  Serial.println(" - TX is writable");
+
+
+  delay(BLE_REQUEST_DELAY); // wait, otherwise writeValue doesn't work for some reason
+                            // to do: fix this ugly hack
+
+  debug(String("BLE: connected"));
+
+  return true;
+}
+
+bool sendCommand(uint8_t *data, uint32_t dataLen){
+  if((pClient!=nullptr)&&(pClient->isConnected())){
+    pRemoteCharacteristic_tx->writeValue(data, dataLen, false);
+    return true;
+  }else{
+    return false;
+  }
+}
+
+bool isPacketValid(byte *packet) //check if packet is valid
+{
+    if (packet == nullptr){
+        return false;
+    }
+
+    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
+    int checksumPos = pHeader->dataLen + 2; // status + data len + data
+
+    int offset = 2; // header 0xDD and command type are not in data length
+
+    if (packet[0] != 0xDD){
+        // start bit missing
+        return false;
+    }
+
+    if (packet[offset + checksumPos + 2] != 0x77){
+        // stop bit missing
+        return false;
+    }
+
+    byte checksum = 0;
+    for (int i = 0; i < checksumPos; i++){
+        checksum += packet[offset + i];
+    }
+    checksum = ((checksum ^ 0xFF) + 1) & 0xFF;
+
+    if (checksum != packet[offset + checksumPos + 1]){
+        return false;
+    }
+
+    return true;
+}
+
+bool processBasicInfo(packBasicInfoStruct *output, byte *data, unsigned int dataLen)
+{
+    // Expected data len
+    if (dataLen != 0x1B)
+    {
+        return false;
+    }
+
+    output->Volts = ((uint32_t)two_ints_into16(data[0], data[1])) * 10; // Resolution 10 mV -> convert to milivolts   eg 4895 > 48950mV
+    output->Amps = ((int32_t)two_ints_into16(data[2], data[3])) * 10;   // Resolution 10 mA -> convert to miliamps
+
+    output->Watts = output->Volts * output->Amps / 1000000; // W
+
+    output->CapacityRemainAh = ((uint16_t)two_ints_into16(data[4], data[5])) * 10;
+    output->CapacityRemainPercent = ((uint8_t)data[19]);
+
+    output->Temp1 = (((uint16_t)two_ints_into16(data[23], data[24])) - 2731);
+    output->Temp2 = (((uint16_t)two_ints_into16(data[25], data[26])) - 2731);
+    output->BalanceCodeLow = (two_ints_into16(data[12], data[13]));
+    output->BalanceCodeHigh = (two_ints_into16(data[14], data[15]));
+    output->MosfetStatus = ((byte)data[20]);
+
+    printBasicInfo();
+
+    return true;
+}
+
+bool processCellInfo(packCellInfoStruct *output, byte *data, unsigned int dataLen)
+{
+    uint16_t _cellSum;
+    uint16_t _cellMin = 5000;
+    uint16_t _cellMax = 0;
+    uint16_t _cellAvg;
+    uint16_t _cellDiff;
+
+    output->NumOfCells = dataLen / 2; // data contains 2 bytes per cell
+
+    //go trough individual cells
+    for (byte i = 0; i < dataLen / 2; i++)
+    {
+        output->CellVolt[i] = ((uint16_t)two_ints_into16(data[i * 2], data[i * 2 + 1])); // Resolution 1 mV
+        _cellSum += output->CellVolt[i];
+        if (output->CellVolt[i] > _cellMax)
+        {
+            _cellMax = output->CellVolt[i];
+        }
+        if (output->CellVolt[i] < _cellMin)
+        {
+            _cellMin = output->CellVolt[i];
+        }
+    }
+
+    output->CellMin = _cellMin;
+    output->CellMax = _cellMax;
+    output->CellDiff = _cellMax - _cellMin;
+    output->CellAvg = _cellSum / output->NumOfCells;
+
+    printCellInfo();
+
+    return true;
+}
+
+bool bmsProcessPacket(byte *packet)
+{
+    bool isValid = isPacketValid(packet);
+
+    if (isValid != true)
+    {
+        Serial.println("Invalid packer received");
+        return false;
+    }
+
+    bmsPacketHeaderStruct *pHeader = (bmsPacketHeaderStruct *)packet;
+    byte *data = packet + sizeof(bmsPacketHeaderStruct); // TODO Fix this ugly hack
+    unsigned int dataLen = pHeader->dataLen;
+
+    bool result = false;
+
+    // find packet type (basic info or cell info)
+    switch (pHeader->type)
+    {
+    case cBasicInfo:
+    {
+        // Process basic info
+        result = processBasicInfo(&packBasicInfo, data, dataLen);
+        if(result==true){
+          ble_packets_received |= 0b01;
+          bms_last_update_time=millis();
+        }
+
+        break;
+    }
+
+    case cCellInfo:
+    {
+        // Process cell info
+        result = processCellInfo(&packCellInfo, data, dataLen);
+        if(result==true){
+          ble_packets_received |= 0b10;
+          bms_last_update_time=millis();
+        }
+        break;
+    }
+
+    default:
+        result = false;
+        Serial.printf("Unsupported packet type detected. Type: %d", pHeader->type);
+    }
+
+    return result;
+}
+
+bool bleCollectPacket(char *data, uint32_t dataSize) // reconstruct packet, called by notifyCallback function
+{
+    static uint8_t packetstate = 0; //0 - empty, 1 - first half of packet received, 2- second half of packet received
+
+    // packet sizes:
+    //  (packet ID 03) = 4 (header) + 23 + 2*N_NTCs + 2 (checksum) + 1 (stop)
+    //  (packet ID 04) = 4 (header) + 2*NUM_CELLS   + 2 (checksum) + 1 (stop)
+    static uint8_t packetbuff[4 + 2*25 + 2 + 1] = {0x0}; // buffer size suitable for up to 25 cells
+
+    static uint32_t totalDataSize = 0;
+    bool retVal = false;
+    hexDump(data,dataSize);
+
+    if(totalDataSize + dataSize > sizeof(packetbuff)){
+      Serial.printf("ERROR: datasize is overlength.");
+
+      debug(
+        String("ERROR: datasize is overlength. ") +
+        String("allocated=") +
+        String(sizeof(packetbuff)) +
+        String(", size=") +
+        String(totalDataSize + dataSize)
+        );
+
+      totalDataSize = 0;
+      packetstate = 0;
+
+      retVal = false;
+    }
+    else if (data[0] == 0xdd && packetstate == 0) // probably got 1st half of packet
+    {
+        Serial.println("PKT1");
+        packetstate = 1;
+        for (uint8_t i = 0; i < dataSize; i++)
+        {
+            packetbuff[i] = data[i];
+        }
+        totalDataSize = dataSize;
+        retVal = true;
+    }
+    else if (data[dataSize - 1] == 0x77 && packetstate == 1) //probably got 2nd half of the packet
+    {
+        Serial.println("PKT2");
+        packetstate = 2;
+        for (uint8_t i = 0; i < dataSize; i++)
+        {
+            packetbuff[i + totalDataSize] = data[i];
+        }
+        totalDataSize += dataSize;
+        retVal = true;
+    }
+
+    if (packetstate == 2) //got full packet
+    {
+        Serial.println("PKT3");
+        uint8_t packet[totalDataSize];
+        memcpy(packet, packetbuff, totalDataSize);
+
+        bmsProcessPacket(packet); //pass pointer to retrieved packet to processing function
+        packetstate = 0;
+        totalDataSize = 0;
+        retVal = true;
+    }
+    return retVal;
+}
+
+bool bmsRequestBasicInfo(){
+    // header status command length data checksum footer
+    //   DD     A5      03     00    FF     FD      77
+    uint8_t data[7] = {0xdd, 0xa5, cBasicInfo, 0x0, 0xff, 0xfd, 0x77};
+    return sendCommand(data, sizeof(data));
+}
+
+bool bmsRequestCellInfo(){
+    // header status command length data checksum footer
+    //  DD      A5      04     00    FF     FC      77
+    uint8_t data[7] = {0xdd, 0xa5, cCellInfo, 0x0, 0xff, 0xfc, 0x77};
+    return sendCommand(data, sizeof(data));
+}
+
+void printBasicInfo() //debug all data to uart
+{
+    Serial.printf("Total voltage: %f\n", (float)packBasicInfo.Volts / 1000);
+    Serial.printf("Amps: %f\n", (float)packBasicInfo.Amps / 1000);
+    Serial.printf("CapacityRemainAh: %f\n", (float)packBasicInfo.CapacityRemainAh / 1000);
+    Serial.printf("CapacityRemainPercent: %d\n", packBasicInfo.CapacityRemainPercent);
+    Serial.printf("Temp1: %f\n", (float)packBasicInfo.Temp1 / 10);
+    Serial.printf("Temp2: %f\n", (float)packBasicInfo.Temp2 / 10);
+    Serial.printf("Balance Code Low: 0x%x\n", packBasicInfo.BalanceCodeLow);
+    Serial.printf("Balance Code High: 0x%x\n", packBasicInfo.BalanceCodeHigh);
+    Serial.printf("Mosfet Status: 0x%x\n", packBasicInfo.MosfetStatus);
+}
+
+void printCellInfo() //debug all data to uart
+{
+    Serial.printf("Number of cells: %u\n", packCellInfo.NumOfCells);
+    for (byte i = 1; i <= packCellInfo.NumOfCells; i++)
+    {
+        Serial.printf("Cell no. %u", i);
+        Serial.printf("   %f\n", (float)packCellInfo.CellVolt[i - 1] / 1000);
+    }
+    Serial.printf("Max cell volt: %f\n", (float)packCellInfo.CellMax / 1000);
+    Serial.printf("Min cell volt: %f\n", (float)packCellInfo.CellMin / 1000);
+    Serial.printf("Difference cell volt: %f\n", (float)packCellInfo.CellDiff / 1000);
+    Serial.printf("Average cell volt: %f\n", (float)packCellInfo.CellAvg / 1000);
+    Serial.println();
+}
+
+void hexDump(const char *data, uint32_t dataSize) //debug function
+{
+    Serial.println("HEX data:");
+
+    for (int i = 0; i < dataSize; i++)
+    {
+        Serial.printf("0x%x, ", data[i]);
+    }
+    Serial.println("");
+}
+
+int16_t two_ints_into16(int highbyte, int lowbyte) // turns two bytes into a single long integer
+{
+    int16_t result = (highbyte);
+    result <<= 8;                //Left shift 8 bits,
+    result = (result | lowbyte); //OR operation, merge the two
+    return result;
+}
diff --git a/EasyFunctions.ino b/EasyFunctions.ino
deleted file mode 100644
index e6d4960..0000000
--- a/EasyFunctions.ino
+++ /dev/null
@@ -1,48 +0,0 @@
-// EasyFunctions - some handy functions that I regularly use
-// by Bas Vermulst
-
-bool InterruptPending(unsigned long *prev_millis, unsigned int period, int mode){
-  // mode = 0: approximate mode without catch-up
-  // mode = 1: exact mode without catch-up
-  // mode = 2: exact mode with catch-up
-  // note: overflow is handled correctly and exactly (tested)
-  
-  if( (millis()-(*prev_millis) > period) || (millis()-(*prev_millis) < 0)){
-    // trigger detected
-    switch(mode){
-      default:
-      case 0:
-        // approximate mode without catch-up
-        *prev_millis=millis();
-        break;
-        
-      case 1:
-        // exact mode without catch-up
-        while(millis()-(*prev_millis) > period){ // unwind
-          *prev_millis=*prev_millis+period;
-        }
-        break;
-        
-      case 2:
-        // exact mode with catch-up
-        *prev_millis=*prev_millis+period;
-        break;
-    }
-      
-    return true;
-  }else{
-    return false;
-  }
-}
-
-
-String IPAddressString(IPAddress address){
-  return String(address[0]) + "." + String(address[1]) + "." + String(address[2]) + "." + String(address[3]);
-}
-
-
-String Float2SciStr(float number, int digits){
-  char char_buffer[40];
-  sprintf(char_buffer,"%.*E", digits, number);
-  return String(char_buffer);
-}