//Reference: https://www.okdo.com/getting-started/get-started-with-arduino-nano-33-ble/
//Reference: https://www.arduino.cc/en/Reference/ArduinoLSM9DS1 //IMU Sensor //https://github.com/arduino-libraries/Arduino_LSM9DS1
//Reference: https://create.arduino.cc/projecthub/monica/getting-started-with-bluetooth-low-energy-ble-ab4c94

//NOTE: Arduino NANO 33 BLE Sense runs at 3.3V (Both digital and analog pins)

#include <ArduinoBLE.h> //ENSURE LATEST VERSION 1.2.0 is installed
#include <Arduino_LSM9DS1.h>

float fgOldX = 0.0;  //previous values
float fgOldY = 0.0;
float fgOldZ = 0.0;

bool bgCentralConnected = false; //connection check
unsigned long lgUpdateTime;      //Loop time interval

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

BLEStringCharacteristic dataCharacteristic("2A56", BLENotify, 22); //www.okdo.com: standard 16-bit characteristic UUID //Size 22 bytes may be modified
                                                                   //App will stream the data on Notify (App Inventor Block 'Register for Strings')

void setup()
{
          Serial.begin(9600); //115200 // initialize serial communication. Virtual serial port to connect with App via BLE
         Serial1.begin(9600); //Second serial port @ the RX0 and TX1 pins to connect with Arduino Serial Monitor //For test phase

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

          BLE.setLocalName("PunchBag");                      //Set name for connection - max 8 characters
          BLE.addService(dataService);                       //Add service
          BLE.setAdvertisedService(dataService);             //Advertise service
          dataService.addCharacteristic(dataCharacteristic); //Add characteristic to service
          BLE.advertise();                                   //Start advertising

          Serial1.print("Peripheral device MAC: ");
          Serial1.println(BLE.address());

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

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

         lgUpdateTime = millis();
}

void loop()
{
         //Execute Comms approx every 250 milli seconds - this can be tweaked during physical trials
	     if(millis() - lgUpdateTime > 250)
         {
               lgUpdateTime = millis();

               if (bgCentralConnected == false)
               {
                         //BLE central (App)
                         BLEDevice central = BLE.central(); //Client App on Phone

                         if (central)
                         {
                              Serial1.print("Connected to Central: ");

                              //the central's BT address:
                              Serial1.println(central.address());
                              bgCentralConnected = true;
                         }

               }

               if (bgCentralConnected == true)
               {
                         updateValue();
               }
               else
               {
                         bgCentralConnected = false;
                         Serial1.print("Disconnected from central: ");
               }
         }
}

void updateValue()
{
//Sending 3 values to App
float fNewX = 0.0;
float fNewY = 0.0;
float fNewZ = 0.0;

         if (IMU.accelerationAvailable())
         {
               IMU.readAcceleration(fNewX, fNewY, fNewZ); // Read new data

               if ( int(fNewX) != int(fgOldX) || int(fNewY) != int(fgOldY) || int(fNewZ) != int(fgOldZ) ) //compare integers (floats are rarely going to be identical)
               {
                         //Send to App
                         Serial.print(fNewX,2);  //Two decimal places
                         Serial.print('|');      //note, tabs were used but they are not recognised in App Inventor
                         Serial.print(fNewY,2);
                         Serial.print('|');
                         Serial.print(fNewZ,2);
                         Serial.println();        //End of BLE Data

                         //save the values for next comparison
                         fgOldX = fNewX;
                         fgOldY = fNewY;
                         fgOldZ = fNewZ;
               }
         }
         else
         {
               Serial1.println("IMU acceleration not available");
         }
}