#include <NimBLEDevice.h>

static const char* TARGET_ADDRESS = "a5:c2:37:2c:05:dc";

static NimBLERemoteCharacteristic* notifyChar = nullptr;
static NimBLERemoteCharacteristic* writeChar = nullptr;

static bool gotNotification = false;

uint8_t JBD_STATUS_REQUEST[] = {
  0xDD, 0xA5, 0x03, 0x00, 0xFF, 0xFD, 0x77
};

void printHex(const uint8_t* data, size_t length) {
  for (size_t i = 0; i < length; i++) {
    if (data[i] < 0x10) Serial.print("0");
    Serial.print(data[i], HEX);
    Serial.print(" ");
  }
  Serial.println();
}

void notifyCallback(
  NimBLERemoteCharacteristic* characteristic,
  uint8_t* data,
  size_t length,
  bool isNotify
) {
  Serial.print("Notification length: ");
  Serial.println(length);

  Serial.print("Raw: ");
  printHex(data, length);

  gotNotification = true;
}

void setup() {
  Serial.begin(115200);
  delay(1000);

  Serial.println();
  Serial.println("=== ESP32 JBD Read Test ===");
  Serial.print("Target: ");
  Serial.println(TARGET_ADDRESS);

  NimBLEDevice::init("OverlandController");
  NimBLEDevice::setPower(ESP_PWR_LVL_P9);

  NimBLEAddress address(std::string(TARGET_ADDRESS), BLE_ADDR_PUBLIC);
  NimBLEClient* client = NimBLEDevice::createClient();

  Serial.println("Connecting...");
  if (!client->connect(address)) {
    Serial.println("Connect failed.");
    return;
  }

  Serial.println("Connected.");

  NimBLERemoteService* service = client->getService("ff00");
  if (!service) {
    Serial.println("Service ff00 not found.");
    client->disconnect();
    return;
  }

  notifyChar = service->getCharacteristic("ff01");
  writeChar = service->getCharacteristic("ff02");

  if (!notifyChar) {
    Serial.println("Notify characteristic ff01 not found.");
    client->disconnect();
    return;
  }

  if (!writeChar) {
    Serial.println("Write characteristic ff02 not found.");
    client->disconnect();
    return;
  }

  Serial.println("Subscribing to notifications...");
  if (!notifyChar->subscribe(true, notifyCallback)) {
    Serial.println("Subscribe failed.");
    client->disconnect();
    return;
  }

  delay(500);

  Serial.print("Sending status request: ");
  printHex(JBD_STATUS_REQUEST, sizeof(JBD_STATUS_REQUEST));

  bool ok = writeChar->writeValue(
    JBD_STATUS_REQUEST,
    sizeof(JBD_STATUS_REQUEST),
    false
  );

  Serial.print("Write result: ");
  Serial.println(ok ? "OK" : "FAILED");

  Serial.println("Waiting for notification...");
}

void loop() {
  if (!gotNotification) {
    delay(100);
    return;
  }

  delay(5000);
}