/*
    Based on Neil Kolban example for IDF: https://github.com/nkolban/esp32-snippets/blob/master/cpp_utils/tests/BLE%20Tests/SampleServer.cpp
    Ported to Arduino ESP32 by Evandro Copercini
    updates by chegewara
*/

#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <Wire.h>


volatile uint8_t data[14]; //センサからのデータ格納用配列
volatile int16_t ax = 0;   //出力データ(生値)
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID        "4fafc201-1fb5-459e-8fcc-c5c9c331914b"
#define CHARACTERISTIC_UUID "beb5483d-36e1-4688-b7f5-ea07361b26a8"



  BLEServer *pServer;
  BLECharacteristic *pCharacteristic;




void setup() {
  Serial.begin(115200);
  Serial.println("Starting BLE work!");
  Wire.begin();//I2C通信を開始する

  Wire.beginTransmission(0x68);//送信処理を開始する(0x68がセンサーのアドレス)
  Wire.write(0x6b);            //レジスタ「0x6b」(動作状変数)を指定
  Wire.write(0x00);            //0x00を指定(ON)
  Wire.endTransmission();      //送信を終了する

  BLEDevice::init("KANNAI_BLE");
  pServer = BLEDevice::createServer();
  BLEService *pService = pServer->createService(SERVICE_UUID);
  pCharacteristic = pService->createCharacteristic(
                                         CHARACTERISTIC_UUID,
                                         BLECharacteristic::PROPERTY_READ |
                                         BLECharacteristic::PROPERTY_WRITE
                                       );

  pCharacteristic->setValue("Hello, Neil");
  pService->start();
  // BLEAdvertising *pAdvertising = pServer->getAdvertising();  // this still is working for backward compatibility
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();
  Serial.println("Characteristic defined! Now you can read it in your phone!");
}

void MPU_DATAGET() {
  Wire.beginTransmission(0x68); //送信処理を開始する
  Wire.write(0x3b);             //(取得値の先頭を指定)
  Wire.endTransmission();       //送信を終了する
  Wire.requestFrom(0x68, 14);   //データを要求する(0x3bから14バイトが6軸の値)

  uint8_t i = 0;
  while (Wire.available()) {
    data[i++] = Wire.read();//データを読み込む
  }

  
  ax = (data[0] << 8) | data[1];//LowとHighを連結して、値を取得する
  
}


void loop() {
  // put your main code here, to run repeatedly:
  MPU_DATAGET();
  Serial.print("ax: ");
  Serial.println(ax);  

  // Convert sensor data to string
  String sensorData = String(ax);
  pCharacteristic->setValue(sensorData.c_str());
  //pCharacteristic->setValue(ax.c_str()); 
  delay(2000);
}