#include <Arduino.h>
#include <Wire.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEAdvertising.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include "DFRobot_BloodOxygen_S.h"

#define I2C_SDA 6
#define I2C_SCL 7
#define I2C_ADDRESS 0x57

#define SERVICE_UUID        "12345678-1234-1234-1234-1234567890ab"
#define CHARACTERISTIC_UUID "12345678-1234-1234-1234-1234567890ac"

DFRobot_BloodOxygen_S_I2C MAX30102(&Wire, I2C_ADDRESS);

BLECharacteristic* pTx = nullptr;
BLEAdvertising* pAdvertising = nullptr;
bool connected = false;
bool sensorReady = false;
uint32_t seqNo = 0;
uint32_t lastSendMs = 0;
uint32_t lastAdvCheckMs = 0;
const uint32_t TX_INTERVAL_MS = 1000;

static int g_lastHr = 0;
static int g_lastSpo2 = 0;
static float g_lastTemp = 0.0f;

class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) override {
    connected = true;
    Serial.println("BLE connected");
  }

  void onDisconnect(BLEServer* pServer) override {
    connected = false;
    Serial.println("BLE disconnected");
    pServer->getAdvertising()->start();
  }
};

bool setupSensor() {
  Wire.begin(I2C_SDA, I2C_SCL);
  for (int attempt = 1; attempt <= 5; attempt++) {
    if (MAX30102.begin()) {
      MAX30102.sensorStartCollect();
      Serial.println("MAX30102 init success");
      return true;
    }
    Serial.println("MAX30102 init fail");
    delay(500);
  }
  Serial.println("MAX30102 unavailable, BLE will advertise with zero readings");
  return false;
}

void setupBLE() {
  BLEDevice::init("BrainFog_Beetle");
  BLEServer* pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService* pService = pServer->createService(SERVICE_UUID);
  pTx = pService->createCharacteristic(
    CHARACTERISTIC_UUID,
    BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY
  );
  pTx->addDescriptor(new BLE2902());
  pTx->setValue("BOOT");
  pService->start();

  pAdvertising = BLEDevice::getAdvertising();
  BLEAdvertisementData advData;
  advData.setFlags(0x06);
  // Keep the legacy advertisement under 31 bytes: 128-bit service UUID in the
  // primary packet, full device name in the active-scan response.
  advData.setCompleteServices(BLEUUID(SERVICE_UUID));
  pAdvertising->setAdvertisementData(advData);

  BLEAdvertisementData scanData;
  scanData.setName("BrainFog_Beetle");
  pAdvertising->setScanResponseData(scanData);
  pAdvertising->setScanResponse(true);
  // Fast, explicit advertising makes the C3 bridge discover the band quickly
  // after power-up instead of waiting many scan cycles.
  pAdvertising->setMinInterval(0x00A0);  // 100 ms, units are 0.625 ms
  pAdvertising->setMaxInterval(0x0140);  // 200 ms
  pAdvertising->setMinPreferred(0x06);
  pAdvertising->setMinPreferred(0x12);

  bool ok = pAdvertising->start();
  Serial.print("Advertising start: ");
  Serial.println(ok ? "OK" : "FAIL");
}

static void updateAdvertisementPayload(uint16_t seq, int hr, int spo2, float tempC) {
  if (pAdvertising == nullptr) return;

  int temp10 = static_cast<int>(tempC * 10.0f + (tempC >= 0 ? 0.5f : -0.5f));
  String mfg;
  mfg.reserve(10);
  mfg += static_cast<char>(0xFF);  // test/company id, little endian
  mfg += static_cast<char>(0xFF);
  mfg += 'B';
  mfg += 'F';
  mfg += static_cast<char>(seq & 0xFF);
  mfg += static_cast<char>((seq >> 8) & 0xFF);
  mfg += static_cast<char>(constrain(hr, 0, 255));
  mfg += static_cast<char>(constrain(spo2, 0, 100));
  mfg += static_cast<char>(temp10 & 0xFF);
  mfg += static_cast<char>((temp10 >> 8) & 0xFF);

  BLEAdvertisementData advData;
  advData.setFlags(0x06);
  advData.setManufacturerData(mfg);
  pAdvertising->stop();
  pAdvertising->setAdvertisementData(advData);
  pAdvertising->start();
}

void setup() {
  pinMode(15, OUTPUT);
  Serial.begin(115200);
  delay(800);
  sensorReady = setupSensor();
  setupBLE();
  Serial.println("Beetle_BLE_TX_MAX30102 ready");
}

void loop() {
  digitalWrite(15, (millis() / 500) % 2 ? LOW : HIGH);

  if (!connected && pAdvertising != nullptr && millis() - lastAdvCheckMs >= 5000) {
    lastAdvCheckMs = millis();
    if (!pAdvertising->isAdvertising()) {
      bool ok = pAdvertising->start();
      Serial.print("Advertising restart: ");
      Serial.println(ok ? "OK" : "FAIL");
    } else {
      Serial.println("Advertising active");
    }
  }

  if (sensorReady) {
    MAX30102.getHeartbeatSPO2();
  }

  if (millis() - lastSendMs >= TX_INTERVAL_MS) {
    lastSendMs = millis();
    seqNo++;

    int hr = sensorReady ? MAX30102._sHeartbeatSPO2.Heartbeat : 0;
    int spo2 = sensorReady ? MAX30102._sHeartbeatSPO2.SPO2 : 0;
    float temp = sensorReady ? MAX30102.getTemperature_C() : 0.0f;

    if (hr < 0) hr = 0;
    if (spo2 < 0) spo2 = 0;
    g_lastHr = hr;
    g_lastSpo2 = spo2;
    g_lastTemp = temp;
    updateAdvertisementPayload(static_cast<uint16_t>(seqNo), g_lastHr, g_lastSpo2, g_lastTemp);

    String payload = "{\"seq\":" + String(seqNo) +
                     ",\"hr\":" + String(hr) +
                     ",\"spo2\":" + String(spo2) +
                     ",\"temp\":" + String(temp, 1) + "}";

    pTx->setValue(payload.c_str());
    if (connected) {
      pTx->notify();
    }

    Serial.println(payload);
  }
}