#include <bluefruit.h>             // Using Bluefruit library instead of ArduinoBLE
#include "LSM6DS3.h"
#include "Wire.h"

// BLE Service Setup (using Bluefruit)
BLEDfu bledfu;  // OTA DFU service
BLEDis bledis;  // Device Information Service
BLEUart bleuart; // UART over BLE for data transmission
BLEBas blebas;   // Battery Service

// Create an instance of the LSM6DS3 IMU
LSM6DS3 myIMU(I2C_MODE, 0x6A);

void setup() {
  Serial.begin(115200);
  
  // Debugging setup
  #if CFG_DEBUG
  while (!Serial) yield();
  #endif

  // Initialize the IMU sensor
  if (myIMU.begin() != 0) {
    Serial.println("IMU error!");
    while (1);
  }

  // Configure Bluefruit
  Bluefruit.autoConnLed(true);  // Enable LED connection indicator
  Bluefruit.configPrphBandwidth(BANDWIDTH_MAX);  // Max bandwidth
  
  // Initialize Bluefruit
  Bluefruit.begin();
  Bluefruit.setTxPower(4);    // Set transmission power
  Bluefruit.Periph.setConnectCallback(connect_callback);
  Bluefruit.Periph.setDisconnectCallback(disconnect_callback);

  // Setup services
  bledfu.begin();  // Device Firmware Update service
  
  // Device Information Service
  bledis.setManufacturer("Adafruit Industries");
  bledis.setModel("Xiao-Accel");
  bledis.begin();
  
  // UART service for data transmission
  bleuart.begin();
  
  // Battery Service
  blebas.begin();
  blebas.write(100);  // Set initial battery level
  
  // Start advertising
  startAdv();
  
  Serial.println("BLE Advertising...");
}

void startAdv(void) {
  // Advertising packet
  Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
  Bluefruit.Advertising.addTxPower();
  
  // Include UART service UUID
  Bluefruit.Advertising.addService(bleuart);
  
  // Secondary scan response packet (device name)
  Bluefruit.ScanResponse.addName();
  
  // Start advertising
  Bluefruit.Advertising.restartOnDisconnect(true);
  Bluefruit.Advertising.setInterval(160, 320);  // in units of 0.625 ms
  Bluefruit.Advertising.setFastTimeout(30);      // 30 seconds
  Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising
}

void loop() {
  // Read accelerometer data
  float accelX = myIMU.readFloatAccelX();
  float accelY = myIMU.readFloatAccelY();
  float accelZ = myIMU.readFloatAccelZ();
  
  // Read gyroscope data
  float gyroX = myIMU.readFloatGyroX();
  float gyroY = myIMU.readFloatGyroY();
  float gyroZ = myIMU.readFloatGyroZ();
  
  // Format accelerometer data string
  char accelBuffer[100];
  snprintf(accelBuffer, sizeof(accelBuffer), "Accelerometer data: X: %.2f Y: %.2f Z: %.2f", accelX, accelY, accelZ);
  
  // Format gyroscope data string
  char gyroBuffer[100];
  snprintf(gyroBuffer, sizeof(gyroBuffer), "Gyroscope data: X: %.2f Y: %.2f Z: %.2f", gyroX, gyroY, gyroZ);
  
  // Send data if connected
  if (Bluefruit.connected()) {
    bleuart.println(accelBuffer);  // Send accelerometer data via BLE UART
    bleuart.println(gyroBuffer);   // Send gyroscope data via BLE UART
  }
  
  Serial.println(accelBuffer);    // Print accelerometer data to serial
  Serial.println(gyroBuffer);     // Print gyroscope data to serial
  delay(100);                     // 100ms delay
}

// Connection callback
void connect_callback(uint16_t conn_handle) {
  BLEConnection* connection = Bluefruit.Connection(conn_handle);
  
  char central_name[32] = { 0 };
  connection->getPeerName(central_name, sizeof(central_name));
  
  Serial.print("Connected to ");
  Serial.println(central_name);
}

// Disconnection callback
void disconnect_callback(uint16_t conn_handle, uint8_t reason) {
  (void) conn_handle;
  (void) reason;
  
  Serial.println();
  Serial.print("Disconnected, reason = 0x");
  Serial.println(reason, HEX);
}