//Nano BLE - UWB Sender Code

#include <ArduinoBLE.h>

 // BLE POS Service
BLEService POSService("11a953ef-0809-4d89-9a56-25ab22841ecd");

// BLE Orientation Characteristic
BLEStringCharacteristic POSSensorData("e624a94b-4a14-41f3-ae53-ec99c6025855", BLERead | BLENotify, 512);

unsigned long previousTime=0;           // end time of the last period in microseconds
unsigned long currentTime=0;
unsigned long deltaTime=0;
const unsigned long period=1000;       // time period for pulse counting in microseconds

String c; //some temporary string variable

char s[60]; //snprintf buffer
String pos;

void setup() 
{
  delay(10000);
  Serial.begin(115200);
  Serial1.begin(115200); //dwm1001 require 115200 baud
  Serial1.write(0x0D); //send the first "Enter"
  Serial1.write(0x0D); //send the second "Enter" to get into Shell Mode
  c = Serial1.readString(); //read the response from dwm1001
  Serial1.write(0x6C); //send "l"
  Serial1.write(0x65); //send "e"
  //Serial1.write(0x63); //send "c" lec shows distance to all anchors & position
  Serial1.write(0x70); //send "p" lep shows only position
  Serial1.write(0x0D); //send "Enter"

  // initialize the built-in LED pin to indicate when a central is connected
  pinMode(LED_BUILTIN, OUTPUT); 

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

  /* Set a local name for the BLE device
     This name will appear in advertising packets
     and can be used by remote devices to identify this BLE device
     The name can be changed but maybe be truncated based on space left in advertisement packet
  */
  BLE.setLocalName("POSSensor");
  BLE.setAdvertisedService(POSService); // add the service UUID
  POSService.addCharacteristic(POSSensorData); // add the POS characteristic
  BLE.addService(POSService); // Add the POS service
  POSSensorData.writeValue("hello"); // set initial value for this characteristic

  /* Start advertising BLE.  It will start continuously transmitting BLE
     advertising packets and will be visible to remote BLE central devices
     until it receives a new connection */

  // start advertising
  BLE.advertise();

  Serial.println("Bluetooth device active, waiting for connections...");  
  String address = BLE.address();
  Serial.print("Local address is: ");
  Serial.println(address);  
  Serial.flush();  
} 

void loop() {
  // wait for a BLE central
  BLEDevice central = BLE.central();

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

    // check the POS every period
    // while the central is connected:
    while (central.connected()) {
      currentTime = micros();
      deltaTime = currentTime-previousTime;   

      /* Check if the time in between samples equals sampling period 
         before taking a new sample */
      if (deltaTime >= period) {
        /* Display the current time in microseconds */
        Serial.print(currentTime);
        Serial.print(",");
        previousTime = currentTime;         

        // Stores each POS data type in a string 'buff' // 
        String buff;
        pos = Serial1.readStringUntil('\n');

        // Sends buff using the BLE connection
        buff += String(pos);
        Serial.println(buff);
        POSSensorData.writeValue(buff);
      }
    }
  }
        
  // when the central disconnects, turn off the LED:
  digitalWrite(LED_BUILTIN, LOW);
  Serial.print("Disconnected from central: ");
  Serial.println(central.address());  
}