#include "BLEDevice.h"

static BLEUUID serviceUUID("0000ff00-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms service
static BLEUUID charUUID_rx("0000ff01-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms rx id
static BLEUUID charUUID_tx("0000ff02-0000-1000-8000-00805f9b34fb"); //xiaoxiang bms tx id

void setup() {
    Serial.begin(115200);
    BLEDevice::init(""); // Initialize BLE device
    Serial.println("Scanning...");
}

void loop() {
    BLEScan* pBLEScan = BLEDevice::getScan(); // Create new scan
    pBLEScan->setActiveScan(true); // Active scan uses more power, but get results faster
    BLEScanResults foundDevices = pBLEScan->start(10); //seconds

    Serial.println("Scanning completed");
    Serial.println("Devices found: " + String(foundDevices.getCount()));

    for (int i = 0; i < foundDevices.getCount(); i++) {
        BLEAdvertisedDevice advertisedDevice = foundDevices.getDevice(i);
        Serial.println("\nFound Device: " + String(advertisedDevice.toString().c_str()));

        if(!advertisedDevice.isAdvertisingService(serviceUUID)) {
            Serial.println("Device does not advertise the specified service UUID.");
            continue;
        }

        BLEClient* pClient = BLEDevice::createClient();
        Serial.println("Connecting to: " + String(advertisedDevice.getAddress().toString().c_str()));

        if(!pClient->connect(&advertisedDevice)) {
            Serial.println("Failed to connect.");
            continue;
        }

        // Get remote service
        BLERemoteService* pRemoteService = pClient->getService(serviceUUID);
        if (pRemoteService == nullptr){
            Serial.println(String("BLE: failed to find service UUID"));
            Serial.print("Failed to find our service UUID: ");
            Serial.println(serviceUUID.toString().c_str());
            pClient->disconnect();
            continue;
        }
        Serial.println(" - Found our service");

        // Get BMS receive characteristic
        BLERemoteCharacteristic* pRemoteCharacteristic_rx = pRemoteService->getCharacteristic(charUUID_rx);
        if (pRemoteCharacteristic_rx == nullptr){
            Serial.println(String("BLE: failed to find RX UUID"));
            Serial.print("Failed to find rx UUID: ");
            Serial.println(charUUID_rx.toString().c_str());
            pClient->disconnect();
            continue;
        }
        Serial.println(" - Found RX characteristic");

        // Register callback for remote characteristic (receive channel)
        if (pRemoteCharacteristic_rx->canNotify()){
            pRemoteCharacteristic_rx->registerForNotify(MyNotifyCallback);
        }else{
            Serial.println(String("BLE: failed to register notification of remote characteristic"));
            Serial.println("Failed to register notification of remote characteristic");
            pClient->disconnect();
            continue;
        }
        Serial.println(" - Registered remote characteristic for notification");

        // Get BMS transmit characteristic
        BLERemoteCharacteristic* pRemoteCharacteristic_tx = pRemoteService->getCharacteristic(charUUID_tx);
        if (pRemoteCharacteristic_tx == nullptr){
            Serial.println(String("BLE: failed to find TX UUID"));
            Serial.print("Failed to find tx UUID: ");
            Serial.println(charUUID_tx.toString().c_str());
            pClient->disconnect();
            continue;
        }
        Serial.println(" - Found TX characteristic");

        // Check whether tx is writeable
        if (!(pRemoteCharacteristic_tx->canWriteNoResponse())){
            Serial.println(String("BLE: failed TX remote characteristic is not writable"));
            Serial.println("Failed TX remote characteristic is not writable");
            pClient->disconnect();
            continue;
        }
        Serial.println(" - TX is writable");

        while(1){
            // REQUEST BASIC INFO
            // header status command length data checksum footer
            //   DD     A5      03     00    FF     FD      77
            delay(500);
            uint8_t a_data[7] = {0xdd, 0xa5, 3, 0x0, 0xff, 0xfd, 0x77};
            pRemoteCharacteristic_tx->writeValue(a_data, sizeof(a_data), false);

            // REQUEST CELL INFO
            // header status command length data checksum footer
            //   DD     A5      03     00    FF     FD      77
            delay(500);
            uint8_t b_data[7] = {0xdd, 0xa5, 4, 0x0, 0xff, 0xfc, 0x77};
            pRemoteCharacteristic_tx->writeValue(b_data, sizeof(b_data), false);
        }
    }
}

static void MyNotifyCallback(BLERemoteCharacteristic *pBLERemoteCharacteristic, uint8_t *pData, size_t length, bool isNotify){
  hexDump((char*)pData, length);
  /*if(!bleCollectPacket((char *)pData, length)){
    Serial.println("ERROR: packet could not be collected.");
}*/
}

void hexDump(const char *data, uint32_t dataSize)
{
    Serial.println("HEX data:");

    for (int i = 0; i < dataSize; i++)
    {
        Serial.printf("0x%x, ", data[i]);
    }
    Serial.println("");
}