#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

char payload[20*2 + 1];
const uint16_t handle = 0x72;

/* Set the delay between fresh samples */
uint16_t BNO055_DELAY = 100;
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 orientationData, angVelocityData, linearAccelData, accelerometerData, gravityData;
  bno.getEvent(&orientationData, Adafruit_BNO055::VECTOR_EULER);
  bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
  bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
  bno.getEvent(&accelerometerData, Adafruit_BNO055::VECTOR_ACCELEROMETER);
  bno.getEvent(&gravityData, Adafruit_BNO055::VECTOR_GRAVITY);
  
  printEvent(&angVelocityData);
  delay(BNO055_DELAY);
}

void printEvent(sensors_event_t* event) {
  double x = -1000000, y = -1000000 , z = -1000000; //dumb values, easy to spot problem
  if (event->type == SENSOR_TYPE_ACCELEROMETER) {
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;
  }
  else if (event->type == SENSOR_TYPE_ORIENTATION) {
    x = event->orientation.x;
    y = event->orientation.y;
    z = event->orientation.z;
  }
  else if (event->type == SENSOR_TYPE_GYROSCOPE) {
    x = event->gyro.x;
    y = event->gyro.y;
    z = event->gyro.z;
  }
  else if (event->type == SENSOR_TYPE_ROTATION_VECTOR) {
    x = event->gyro.x;
    y = event->gyro.y;
    z = event->gyro.z;
  }
  else if (event->type == SENSOR_TYPE_LINEAR_ACCELERATION) {
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;
  }
  else if (event->type == SENSOR_TYPE_GRAVITY) {
    x = event->acceleration.x;
    y = event->acceleration.y;
    z = event->acceleration.z;
    
  }

  double totalSpin = abs(x) + abs(y) + abs(z);
  debugSerial.println(totalSpin);

  String num2String = " ";
  num2String += totalSpin;
  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);
  
}