#include <RN487x_BLE.h>
#include <Adafruit_BNO055.h>

#if __cplusplus >= 201402L
void operator delete(void* ptr, size_t size) noexcept {
  operator delete(ptr);
}
void operator delete[](void * ptr, size_t size) noexcept {
  operator delete[](ptr);
}
#endif

#define debugSerial Serial 
#define bleSerial Serial1
#define window_size 25

const uint16_t BNO055_DELAY = 10;
const uint16_t handle = 0x72;

char payload[20*2 + 1];
int index = 0;
double sum = 0;
double readings[window_size];
double averaged = 0;
double peak = 0;
int lastLaunchTime = millis();
int currentLaunchTime = 0;


Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);

void setup(){
  
  debugSerial.begin(115200);

  // Wait for PC to connect, give up after SERIAL_TIMEOUT_MS
  while ((!debugSerial) && (millis() < 5000));
  
  rn487xBle.setDiag(debugSerial);
  rn487xBle.hwInit();
  bleSerial.begin(rn487xBle.getDefaultBaudRate());
  rn487xBle.initBleStream(&bleSerial);

  if (rn487xBle.swInit()) {
    debugSerial.println("RN4871 initialized");
    rn487xBle.enterCommandMode();
  }
  else {
    debugSerial.println("RN4871 failed");
    while(1);
  }

  if(bno.begin()) {
    debugSerial.println("BN0O55 initialized");
  }
  else {
    debugSerial.println("BN0O55 failed");
  }

  delay(1000);
  
}

void loop(void){

  sensors_event_t  linearAccelData;
  bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
  printEvent(&linearAccelData);
  delay(BNO055_DELAY);
  
}

void printEvent(sensors_event_t* event) {
  
  double x = event->acceleration.x;
  double y = event->acceleration.y;
  double z = event->acceleration.z;

  double accel = sqrt(pow(x,2) + pow(y,2) + pow(z,2));

  sum = sum - readings[index];       // Remove the oldest entry from the sum
  readings[index] = accel;           // Add the newest reading to the window
  sum = sum + accel;                 // Add the newest reading to the sum
  index = (index+1) % window_size;   // Increment the index, and wrap to 0 if it exceeds the window size

  averaged = sum / window_size;      // Divide the sum of the window by the window size for the result

  #define low 4
  #define launch 8

  // if you set a new peak outside low level, set a new peak
  if(averaged > peak & averaged > low){
    peak = averaged;  
  }
  // if you drop back down to low level ...
  else if(averaged < low){
    
    // ... send the peak over BLE, if it's high enough
    // and at least 100ms have elapsed since you last sent
    currentLaunchTime = millis();
    
    if(peak > launch & (currentLaunchTime - lastLaunchTime > 100)){
      
      debugSerial.println(peak);
      String num2String = " ";
      num2String += peak;
      
      char outputData[20];
      num2String.toCharArray(outputData, 20);
    
      for (int i = 0, j = 0; i < strlen(outputData); ++i, j += 2) {
        sprintf(payload + j, "%02x", outputData[i] & 0xff);
      }
      
      rn487xBle.writeLocalCharacteristic(handle,payload);
      lastLaunchTime = currentLaunchTime;
    }

    // reset the peak
    peak = 0;
  }
}