#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include <BLEClient.h>
#include <BLERemoteService.h>
#include <BLERemoteCharacteristic.h>

static const char* TARGET_NAME = "BrainFog_Beetle";
static BLEUUID SERVICE_UUID("12345678-1234-1234-1234-1234567890ab");
static BLEUUID CHAR_TX_UUID("12345678-1234-1234-1234-1234567890ac");

static BLEAdvertisedDevice* targetDevice = nullptr;
static BLEScan* pBLEScan = nullptr;
static BLEClient* pClient = nullptr;
static BLERemoteCharacteristic* pRemoteTx = nullptr;

static bool doConnect = false;
static bool connected = false;
static bool doScan = true;

volatile bool hasNewPacket = false;
String packetBuf = "";

int lastHR = -1;
int lastSpO2 = -1;
float lastTemp = -1.0f;
int lastSeq = -1;

int readIntField(const String& s, const String& key, int defVal) {
  int k = s.indexOf("\"" + key + "\":");
  if (k < 0) return defVal;
  int st = s.indexOf(':', k);
  if (st < 0) return defVal;
  st += 1;
  while (st < (int)s.length() && (s[st] == ' ' || s[st] == '\"')) st++;
  int ed = st;
  while (ed < (int)s.length() && (isDigit(s[ed]) || s[ed] == '-')) ed++;
  if (ed <= st) return defVal;
  return s.substring(st, ed).toInt();
}

float readFloatField(const String& s, const String& key, float defVal) {
  int k = s.indexOf("\"" + key + "\":");
  if (k < 0) return defVal;
  int st = s.indexOf(':', k);
  if (st < 0) return defVal;
  st += 1;
  while (st < (int)s.length() && (s[st] == ' ' || s[st] == '\"')) st++;
  int ed = st;
  while (ed < (int)s.length() && (isDigit(s[ed]) || s[ed] == '-' || s[ed] == '.')) ed++;
  if (ed <= st) return defVal;
  return s.substring(st, ed).toFloat();
}

static void notifyCallback(
  BLERemoteCharacteristic* pBLERemoteCharacteristic,
  uint8_t* pData,
  size_t length,
  bool isNotify
) {
  String msg;
  msg.reserve(length);
  for (size_t i = 0; i < length; i++) {
    char c = (char)pData[i];
    if (c >= 32 && c <= 126) msg += c;
    else msg += '.';
  }
  packetBuf = msg;
  hasNewPacket = true;
}

class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks {
  void onResult(BLEAdvertisedDevice advertisedDevice) override {
    if (advertisedDevice.haveName() && advertisedDevice.getName() == TARGET_NAME) {
      targetDevice = new BLEAdvertisedDevice(advertisedDevice);
      doConnect = true;
      doScan = false;
      pBLEScan->stop();
    }
  }
};

bool connectToServer() {
  if (targetDevice == nullptr) return false;

  pClient = BLEDevice::createClient();
  if (!pClient->connect(targetDevice)) return false;

  BLERemoteService* pRemoteService = pClient->getService(SERVICE_UUID);
  if (pRemoteService == nullptr) {
    pClient->disconnect();
    return false;
  }

  pRemoteTx = pRemoteService->getCharacteristic(CHAR_TX_UUID);
  if (pRemoteTx == nullptr) {
    pClient->disconnect();
    return false;
  }

  if (pRemoteTx->canNotify()) {
    pRemoteTx->registerForNotify(notifyCallback);
  }

  connected = true;
  return true;
}

void setup() {
  Serial.begin(115200);
  delay(1200);
  Serial.println("WROOM BLE RX start");

  BLEDevice::init("BrainFog_WROOM");
  pBLEScan = BLEDevice::getScan();
  pBLEScan->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks());
  pBLEScan->setInterval(1349);
  pBLEScan->setWindow(449);
  pBLEScan->setActiveScan(true);
}

void loop() {
  if (doScan) {
    Serial.println("Scanning...");
    pBLEScan->start(2, false);
  }

  if (doConnect) {
    if (connectToServer()) {
      Serial.println("Connected to Beetle BLE");
    } else {
      Serial.println("Connect failed, rescan...");
      doScan = true;
    }
    doConnect = false;
  }

  if (connected && pClient && !pClient->isConnected()) {
    connected = false;
    Serial.println("BLE disconnected, rescan...");
    doScan = true;
  }

  if (hasNewPacket) {
    hasNewPacket = false;

    lastSeq = readIntField(packetBuf, "seq", lastSeq);
    lastHR = readIntField(packetBuf, "hr", lastHR);
    lastSpO2 = readIntField(packetBuf, "spo2", lastSpO2);
    lastTemp = readFloatField(packetBuf, "temp", lastTemp);

    Serial.print("RX RAW: ");
    Serial.println(packetBuf);

    Serial.print("SEQ=");
    Serial.print(lastSeq);
    Serial.print(" HR=");
    Serial.print(lastHR);
    Serial.print(" SPO2=");
    Serial.print(lastSpO2);
    Serial.print(" TEMP=");
    Serial.println(lastTemp, 1);
  }

  delay(20);
}