#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_ADXL345_U.h>
#include <NimBLEDevice.h>

// ================== I2C PINS (CHANGE THESE) ==================
static const int I2C_SDA = 4;   // <-- change to your wiring
static const int I2C_SCL = 3;   // <-- change to your wiring

// ================== ADXL345 ==================
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);

// ================== BLE UUIDS ==================
static const char* BLE_DEVICE_NAME = "ADXL345-C6";
static NimBLEUUID SERVICE_UUID("7e2a1001-1111-2222-3333-444455556666");
static NimBLEUUID CHAR_UUID   ("7e2a1002-1111-2222-3333-444455556666");

NimBLEServer* pServer = nullptr;
NimBLECharacteristic* pChar = nullptr;
volatile bool deviceConnected = false;

class ServerCallbacks : public NimBLEServerCallbacks {
  void onConnect(NimBLEServer* s, NimBLEConnInfo& connInfo) override {
    deviceConnected = true;
    Serial.println("BLE: Connected");
  }
  void onDisconnect(NimBLEServer* s, NimBLEConnInfo& connInfo, int reason) override {
    deviceConnected = false;
    Serial.println("BLE: Disconnected");
    NimBLEDevice::startAdvertising();
  }
};

void setupBLE() {
  NimBLEDevice::init(BLE_DEVICE_NAME);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9);

  pServer = NimBLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());

  NimBLEService* pService = pServer->createService(SERVICE_UUID);

  pChar = pService->createCharacteristic(
    CHAR_UUID,
    NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY
  );
  pChar->createDescriptor("2902"); // CCCD for notifications

  pService->start();

  NimBLEAdvertising* adv = NimBLEDevice::getAdvertising();
  adv->addServiceUUID(SERVICE_UUID);
  adv->start();

  Serial.println("BLE: Advertising started");
}

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

  Serial.println("\nESP32-C6 ADXL345 BLE starting...");

  Wire.begin(I2C_SDA, I2C_SCL);

  if (!accel.begin()) {
    Serial.println("ERROR: ADXL345 not detected. Check wiring/power/address.");
    while (true) delay(1000);
  }

  // Range options: ADXL345_RANGE_2_G, 4_G, 8_G, 16_G
  accel.setRange(ADXL345_RANGE_4_G);

  // Data rate options: ADXL345_DATARATE_100_HZ, 50_HZ, 25_HZ, etc.
  accel.setDataRate(ADXL345_DATARATE_50_HZ);

  setupBLE();
}

void loop() {
  // ~20 Hz notify
  static uint32_t lastMs = 0;
  if (millis() - lastMs < 50) return;
  lastMs = millis();

  sensors_event_t event;
  accel.getEvent(&event);

  // Adafruit returns m/s^2. Convert to g.
  const float g = 9.80665f;
  float ax_g = event.acceleration.x / g;
  float ay_g = event.acceleration.y / g;
  float az_g = event.acceleration.z / g;

  // CSV payload
  char payload[64];
  snprintf(payload, sizeof(payload), "%.4f,%.4f,%.4f", ax_g, ay_g, az_g);

  Serial.print("TX: ");
  Serial.println(payload);

  pChar->setValue((uint8_t*)payload, strlen(payload));
  if (deviceConnected) {
    pChar->notify();
  }
}
