#include <ArduinoBLE.h>

// Define a unique UUID for the Service and Characteristic
// You can generate your own at https://www.uuidgenerator.net/
const char* deviceServiceUuid = "19B10000-E8F2-537E-4F6C-D104768A1214";
const char* deviceCharacteristicUuid = "19B10001-E8F2-537E-4F6C-D104768A1214";

constexpr size_t ARRAY_SIZE_BYTES = 244;
constexpr uint32_t COUNTER_INTERVAL = 0x100;
constexpr uint32_t COUNTER_INTERVAL_MASK = (COUNTER_INTERVAL-1);
constexpr uint32_t BITS_PER_BYTE = 8;
constexpr uint32_t MILLISECONDS_PER_SECOND = 1000;
constexpr uint32_t BYTES_SENT_PER_INTERVAL = (ARRAY_SIZE_BYTES*COUNTER_INTERVAL);

// Create the BLE Service
BLEService sensorService(deviceServiceUuid);

BLECharacteristic largeDataChar(deviceCharacteristicUuid, BLERead | BLENotify, 244);

int lastValue = 0;
long previousMillis = 0;

void setup() {

  Serial.begin(9600);
  while (!Serial);

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

 // not supported
 // if (BLE.setMTU(247)) {
 //   Serial.print("MTU set to: ");
 //   Serial.println(BLE.mtu());
 // } else {
 //   Serial.println("Failed to set MTU");
 // }

  // Set the local name that will appear on your phone
  BLE.setLocalName("UnoR4_Sensor");
  BLE.setAdvertisedService(sensorService);

  // Add the characteristic to the service
  sensorService.addCharacteristic(largeDataChar);

  // Add the service
  BLE.addService(sensorService);

  // Start advertising
  BLE.advertise();
  Serial.println("BLE Peripheral active, waiting for connections...");
}

void loop() {
  int count = 0;
  long currentMillis = millis();
  long previousMillis;
  uint8_t largePacket[ARRAY_SIZE_BYTES];

  // Listen for BLE centrals to connect
  BLEDevice central = BLE.central();

  if (central) {
    Serial.print("Connected to central: ");
    Serial.println(central.address());

    while (central.connected()) {
        // This line triggers the notification to the connected device
        //sensorLevelChar.writeValue(newValue);
        largeDataChar.writeValue(largePacket, ARRAY_SIZE_BYTES);
        if ((count++ & COUNTER_INTERVAL_MASK) == 0) {
           previousMillis = currentMillis;
           currentMillis = millis();
           Serial.println((BYTES_SENT_PER_INTERVAL*MILLISECONDS_PER_SECOND)/(float)(currentMillis - previousMillis));
        }
    }

    Serial.print("Disconnected from central: ");
    Serial.println(central.address());
  }
}