#include <ArduinoBLE.h>
#include <Arduino_LSM9DS1.h>

BLEService dataService("180C"); // User defined service

BLEStringCharacteristic dataCharacteristic("2A56", // standard 16-bit characteristic UUID
    BLERead | BLENotify, 50); // remote clients will be able to read and subscribe to notifications

float oldX = 0.0;  // last value
float oldY = 0.0;
float oldZ = 0.0;
long previousMillis = 0;  // last time the value was checked, in ms

void setup()
{
    Serial.begin(9600); // initialize serial communication
    while (!Serial)
        ;

    pinMode(LED_BUILTIN, OUTPUT); // initialize the built-in LED pin

    if (!BLE.begin()) { // initialize BLE
        Serial.println("starting BLE failed!");
        while (1)
            ;
    }

    BLE.setLocalName("Sac de frappe"); // Set name for connection
    BLE.setAdvertisedService(dataService); // Advertise service
    dataService.addCharacteristic(dataCharacteristic); // Add characteristic to service
    BLE.addService(dataService); // Add service
    dataCharacteristic.setValue(String(oldX)); // Set data string

    BLE.advertise(); // Start advertising
    Serial.print("Peripheral device MAC: ");
    Serial.println(BLE.address());
    Serial.println("Waiting for connections...");

    Serial.begin(9600);
    while (!Serial);
    Serial.println("Started");

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

    Serial.print("Accelerometer sample rate = ");
    Serial.print(IMU.accelerationSampleRate());
    Serial.println(" Hz");
    Serial.println();
    Serial.println("Acceleration in G's");
    Serial.println("X\tY\tZ");
}

void loop()
{
    BLEDevice central = BLE.central(); // Wait for a BLE central to connect

    // if a central is connected to the peripheral:
    if (central) {
        Serial.print("Connected to central MAC: ");
        // print the central's BT address:
        Serial.println(central.address());
        // turn on the LED to indicate the connection:
        digitalWrite(LED_BUILTIN, HIGH);

        // update value every 200ms
        // while the central is connected:
        while (central.connected()) {
            long currentMillis = millis();
            // if 200ms have passed, update value:
            if (currentMillis - previousMillis >= 200) {
                previousMillis = currentMillis;
                updateValue();
            }
        }
    }
    // when the central disconnects, turn off the LED:
    digitalWrite(LED_BUILTIN, LOW);
    Serial.print("Disconnected from central MAC: ");
    Serial.println(central.address());
}

void updateValue() {
    float x, y, z;

    if (!IMU.accelerationAvailable()) return; // Return if not ready
    IMU.readAcceleration(x, y, z); // Read new data

    if (x != oldX || y != oldY || z != oldZ) {
        // print it
        Serial.print(x);
        Serial.print('\t');
        Serial.print(y);
        Serial.print('\t');
        Serial.println(z);
        dataCharacteristic.writeValue((String(x)));  // update value
        // save the value for next comparison
        oldX = x;
        oldY = y;
        oldZ = z;
    }
}