#include <ArduinoBLE.h>
#include <Arduino_LSM6DS3.h>
// 服務名稱
BLEService myService("19B10000-E8F2-537E-4F6C-D104768A1214");
// LED 與 RSSI
BLEIntCharacteristic myServiceLED ("19B10001-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
BLEIntCharacteristic myServicerssi("19B10002-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
// 三軸加速器
BLEFloatCharacteristic myServiceAccX("19B10011-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
BLEFloatCharacteristic myServiceAccY("19B10012-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
BLEFloatCharacteristic myServiceAccZ("19B10013-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
// 陀螺儀
BLEFloatCharacteristic myServiceGyrX("19B10021-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
BLEFloatCharacteristic myServiceGyrY("19B10022-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
BLEFloatCharacteristic myServiceGyrZ("19B10023-E8F2-537E-4F6C-D104768A1214", BLERead | BLEWrite);
float x, y, z;

void setup() {
  // 設定好序列埠，以及 LED 腳位
  Serial.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
  
  // 初始化 BLE 藍芽
  if (!BLE.begin()) {
    Serial.println("starting Bluetooth® Low Energy module failed!");
    while (1);
  }

  // 初始化 IMU
  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1);
  }

  // 設定藍芽本身名稱顯示，並設定好要傳輸的資料，本例則是 myServiceChar1
  BLE.setLocalName("Nano 33 IoT VBird");
  BLE.setAdvertisedService(myService);
  myService.addCharacteristic(myServicerssi);
  myService.addCharacteristic(myServiceAccX);
  myService.addCharacteristic(myServiceAccY);
  myService.addCharacteristic(myServiceAccZ);
  myService.addCharacteristic(myServiceGyrX);
  myService.addCharacteristic(myServiceGyrY);
  myService.addCharacteristic(myServiceGyrZ);
  BLE.addService(myService);

  // set the initial value for the characeristic:
  myServiceLED.writeValue(0);
  myServicerssi.writeValue(0);
  myServiceAccX.writeValue(0.0);
  myServiceAccY.writeValue(0.0);
  myServiceAccZ.writeValue(0.0);
  myServiceGyrX.writeValue(0.0);
  myServiceGyrY.writeValue(0.0);
  myServiceGyrZ.writeValue(0.0);
  
  // 開始廣播上述資料
  BLE.advertise();
  Serial.println("BLE Peripheral started");
}

void loop() {
  // listen for Bluetooth® Low Energy peripherals to connect:
  BLEDevice central = BLE.central();

  // if a central is connected to peripheral:
  if (central) {
    Serial.print("Connected to central: ");
    // print the central's MAC address:
    Serial.println(central.address());
    // while the central is still connected to peripheral:
    while (central.connected()) {
      digitalWrite(LED_BUILTIN, HIGH);
      int rssi = central.rssi();
      myServicerssi.writeValue(rssi);
      if (IMU.accelerationAvailable()) {
        IMU.readAcceleration(x, y, z);
        myServiceAccX.writeValue(x);
        myServiceAccY.writeValue(y);
        myServiceAccZ.writeValue(z);
      }
      if (IMU.gyroscopeAvailable()) {
        IMU.readGyroscope(x, y, z);
        myServiceGyrX.writeValue(x);
        myServiceGyrY.writeValue(y);
        myServiceGyrZ.writeValue(z);
      }
    }

    // when the central disconnects, print it out:
    digitalWrite(LED_BUILTIN, LOW);
    Serial.print(F("Disconnected from central: "));
    Serial.println(central.address());
  }
}