#include <Arduino.h>
#include <Wire.h>
#include <cmath>
#include <Adafruit_BME680.h>
#include <NimBLEDevice.h>
#include <NimBLEAdvertisementData.h>



namespace {

#define I2C_SDA_PIN 21
#define I2C_SCL_PIN 22
#define BME680_I2C_ADDR 0x77

constexpr char kDeviceName[] = "ESP32C6 ESS";
const NimBLEUUID kEnvironmentalServiceUUID((uint16_t)0x181A);
const NimBLEUUID kTemperatureCharUUID((uint16_t)0x2A6E);
const NimBLEUUID kHumidityCharUUID((uint16_t)0x2A6F);
const NimBLEUUID kPressureCharUUID((uint16_t)0x2A6D);
constexpr uint32_t kSecurityPasskey = 654321; // 6-digit static passkey for pairing
constexpr uint32_t kNotifyIntervalMs = 5000;  // send notify every 5 seconds

Adafruit_BME680 g_bme;
bool g_bmeReady = false;
NimBLECharacteristic* g_temperatureCharacteristic = nullptr;
NimBLECharacteristic* g_humidityCharacteristic = nullptr;
NimBLECharacteristic* g_pressureCharacteristic = nullptr;
volatile bool g_isConnected = false;
uint32_t g_lastNotify = 0;

class ServerCallbacks : public NimBLEServerCallbacks {
  void onConnect(NimBLEServer* server, NimBLEConnInfo& connInfo) override {
    g_isConnected = true;
    Serial.printf("[BLE] Central connected: %s\n", connInfo.getAddress().toString().c_str());
  }

  void onDisconnect(NimBLEServer* server, NimBLEConnInfo& connInfo, int reason) override {
    g_isConnected = false;
    Serial.printf("[BLE] Central disconnected (reason %d)\n", reason);
    if (!NimBLEDevice::startAdvertising()) {
      Serial.println("[BLE] Failed to restart advertising");
    }
  }

  uint32_t onPassKeyDisplay() override {
    Serial.printf("[BLE] Displaying passkey: %06u\n", kSecurityPasskey);
    return kSecurityPasskey;
  }

  void onConfirmPassKey(NimBLEConnInfo& connInfo, uint32_t pin) override {
    Serial.printf("[BLE] Confirm passkey %06u for %s\n", pin, connInfo.getAddress().toString().c_str());
    NimBLEDevice::injectConfirmPasskey(connInfo, true);
  }

  void onAuthenticationComplete(NimBLEConnInfo& connInfo) override {
    if (connInfo.isEncrypted()) {
      Serial.printf("[BLE] Link encrypted; bonded=%s, authenticated=%s\n",
                    connInfo.isBonded() ? "yes" : "no",
                    connInfo.isAuthenticated() ? "yes" : "no");
    } else {
      Serial.printf("[BLE] Authentication failed; disconnecting %s\n", connInfo.getAddress().toString().c_str());
      NimBLEDevice::getServer()->disconnect(connInfo.getConnHandle());
    }
  }
};

class ValueCallbacks : public NimBLECharacteristicCallbacks {
  void onRead(NimBLECharacteristic* characteristic, NimBLEConnInfo& connInfo) override {
    Serial.printf("[GATT] Read from %s on characteristic %s\n",
                  connInfo.getAddress().toString().c_str(),
                  characteristic->getUUID().toString().c_str());
  }

  void onWrite(NimBLECharacteristic* characteristic, NimBLEConnInfo& connInfo) override {
    std::string value = characteristic->getValue();
    Serial.printf("[GATT] Write to %s from %s, %zu bytes\n",
                  characteristic->getUUID().toString().c_str(),
                  connInfo.getAddress().toString().c_str(),
                  value.size());
  }
};

ServerCallbacks g_serverCallbacks;
ValueCallbacks g_valueCallbacks;

bool initBME680() {
  if (!g_bme.begin(BME680_I2C_ADDR)) {
    Serial.println("[BME680] Not found on 0x77, trying 0x76...");
    if (!g_bme.begin(0x76)) {
      Serial.println("[BME680] Sensor not found. Check wiring/power.");
      return false;
    }
  }

  g_bme.setTemperatureOversampling(BME680_OS_8X);
  g_bme.setHumidityOversampling(BME680_OS_2X);
  g_bme.setPressureOversampling(BME680_OS_4X);
  g_bme.setIIRFilterSize(BME680_FILTER_SIZE_3);
  g_bme.setGasHeater(0, 0); // Disable gas heater for periodic environmental sampling

  Serial.println("[BME680] Sensor initialized");
  return true;
}

bool readBME680(float& temperatureC, float& humidityPct, float& pressurePa) {
  if (!g_bme.performReading()) {
    Serial.println("[BME680] Failed to perform reading");
    return false;
  }

  temperatureC = g_bme.temperature;
  humidityPct = g_bme.humidity;
  pressurePa = g_bme.pressure; // Library returns Pascals
  return true;
}

void setupSecurity() {
  NimBLEDevice::setSecurityAuth(true, true, true); // bonding, MITM, secure connections
  NimBLEDevice::setSecurityIOCap(BLE_HS_IO_DISPLAY_YESNO);
  NimBLEDevice::setSecurityPasskey(kSecurityPasskey);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9);
}

void setupGattServer() {
  NimBLEServer* server = NimBLEDevice::createServer();
  server->setCallbacks(&g_serverCallbacks, false);

  NimBLEService* environmentalService = server->createService(kEnvironmentalServiceUUID);

  g_temperatureCharacteristic = environmentalService->createCharacteristic(
      kTemperatureCharUUID,
      NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_ENC |
          NIMBLE_PROPERTY::READ_AUTHEN |
          NIMBLE_PROPERTY::NOTIFY);
  g_temperatureCharacteristic->setCallbacks(&g_valueCallbacks);
  uint8_t tempPlaceholder[2] = {0xFF, 0x7F}; // NaN equivalent (0x7FFF)
  g_temperatureCharacteristic->setValue(tempPlaceholder, sizeof(tempPlaceholder));

  g_humidityCharacteristic = environmentalService->createCharacteristic(
      kHumidityCharUUID,
      NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_ENC |
          NIMBLE_PROPERTY::READ_AUTHEN |
          NIMBLE_PROPERTY::NOTIFY);
  g_humidityCharacteristic->setCallbacks(&g_valueCallbacks);
  uint8_t humidityPlaceholder[2] = {0xFF, 0xFF};
  g_humidityCharacteristic->setValue(humidityPlaceholder, sizeof(humidityPlaceholder));

  g_pressureCharacteristic = environmentalService->createCharacteristic(
      kPressureCharUUID,
      NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_ENC |
          NIMBLE_PROPERTY::READ_AUTHEN |
          NIMBLE_PROPERTY::NOTIFY);
  g_pressureCharacteristic->setCallbacks(&g_valueCallbacks);
  uint8_t pressurePlaceholder[4] = {0xFF, 0xFF, 0xFF, 0xFF};
  g_pressureCharacteristic->setValue(pressurePlaceholder, sizeof(pressurePlaceholder));

  environmentalService->start();

  NimBLEAdvertising* advertising = NimBLEDevice::getAdvertising();
  NimBLEAdvertisementData advData;
  advData.setName(kDeviceName);
  advData.addServiceUUID(kEnvironmentalServiceUUID);
  advData.setAppearance(0x0341); // Thermometer
  advertising->setAdvertisementData(advData);

  NimBLEAdvertisementData scanData;
  scanData.setName("Env Sensing", false);
  advertising->setScanResponseData(scanData);

  advertising->setConnectableMode(BLE_GAP_CONN_MODE_UND);
  advertising->setDiscoverableMode(BLE_GAP_DISC_MODE_GEN);

  if (advertising->start()) {
    Serial.println("[BLE] Advertising environmental sensing service");
  } else {
    Serial.println("[BLE] Failed to start advertising");
  }
}

void updateEnvironmentalMeasurements() {
  if (!g_isConnected || !g_bmeReady ||
      g_temperatureCharacteristic == nullptr ||
      g_humidityCharacteristic == nullptr ||
      g_pressureCharacteristic == nullptr) {
    return;
  }

  const uint32_t now = millis();
  if (now - g_lastNotify < kNotifyIntervalMs) {
    return;
  }
  g_lastNotify = now;

  float temperatureC = 0.0f;
  float humidityPct = 0.0f;
  float pressurePa = 0.0f;
  if (!readBME680(temperatureC, humidityPct, pressurePa)) {
    return;
  }

  const int16_t temperatureHundredths =
      static_cast<int16_t>(std::lround(temperatureC * 100.0));
  const uint16_t humidityHundredths =
      static_cast<uint16_t>(std::lround(humidityPct * 100.0));
  const uint32_t pressureValue = static_cast<uint32_t>(std::lround(pressurePa));

  uint8_t temperaturePayload[2] = {
      static_cast<uint8_t>(temperatureHundredths & 0xFF),
      static_cast<uint8_t>((temperatureHundredths >> 8) & 0xFF),
  };
  g_temperatureCharacteristic->setValue(temperaturePayload, sizeof(temperaturePayload));
  g_temperatureCharacteristic->notify();

  uint8_t humidityPayload[2] = {
      static_cast<uint8_t>(humidityHundredths & 0xFF),
      static_cast<uint8_t>((humidityHundredths >> 8) & 0xFF),
  };
  g_humidityCharacteristic->setValue(humidityPayload, sizeof(humidityPayload));
  g_humidityCharacteristic->notify();

  uint8_t pressurePayload[4] = {
      static_cast<uint8_t>(pressureValue & 0xFF),
      static_cast<uint8_t>((pressureValue >> 8) & 0xFF),
      static_cast<uint8_t>((pressureValue >> 16) & 0xFF),
      static_cast<uint8_t>((pressureValue >> 24) & 0xFF),
  };
  g_pressureCharacteristic->setValue(pressurePayload, sizeof(pressurePayload));
  g_pressureCharacteristic->notify();

  Serial.printf("[ESS] Notified T=%.2fC H=%.2f%% P=%.2fhPa\n",
                temperatureC,
                humidityPct,
                pressurePa / 100.0f);
}

} // namespace

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    delay(10);
  }
  delay(1000);
  Serial.println();
  Serial.println("[BOOT] ESP32-C6 Environmental Sensing Server");

  Wire.begin(I2C_SDA_PIN, I2C_SCL_PIN);
  g_bmeReady = initBME680();
  if (!g_bmeReady) {
    Serial.println("[BOOT] BME680 not ready; characteristics will remain invalid");
  }

  NimBLEDevice::init(kDeviceName);
  setupSecurity();
  setupGattServer();
}

void loop() {
  updateEnvironmentalMeasurements();
  delay(100);
}