//====================BNO-08X=======================
#include <Adafruit_BNO08x.h>
#define BNO08X_CS 10
#define BNO08X_INT 9
#define BNO08X_RESET -1

Adafruit_BNO08x  bno08x(BNO08X_RESET);
sh2_SensorValue_t sensorValue;

double a_constant = 9.80665; //gravitational constant
double a_range = 9; //device range (e.g. 16 means +/- 16g)
double a_max = a_constant*a_range;
// double a_max_mag = sqrt(3*a_max*a_max);

double g_max = 2000; //device range (e.g. 2000 means +/- 2000 °/s)
// double g_max_mag = sqrt(3*g_max*g_max);

class Data {
public:
  float channel, ax, ay, az, a, gx, gy, gz, g, ox, oy, oz;
};
Data data;
bool send;
//====================BNO-08X=======================


void setup(void) {
  Serial.begin(1000000);

  //====================BNO-08X=======================
  while (!bno08x.begin_I2C()) {
    delay(500);
  }

  Serial.println("BNO08x Found!");


  if (!bno08x.enableReport(SH2_LINEAR_ACCELERATION)) {
    Serial.println("Could not enable linear acceleration");
  }
  // if (!bno08x.enableReport(SH2_GYROSCOPE_CALIBRATED)) {
  //   Serial.println("Could not enable gyroscope");
  // }
  if (!bno08x.enableReport(SH2_GRAVITY)) {
    Serial.println("Could not enable gravity");
  }
  //====================BNO-08X=======================
}


void loop() {
  //====================BNO-08X=======================
  while (!bno08x.getSensorEvent(&sensorValue)) {}


  switch (sensorValue.sensorId) {
    case SH2_LINEAR_ACCELERATION:
      data.ax = sensorValue.un.linearAcceleration.x/a_max;
      data.ay = sensorValue.un.linearAcceleration.y/a_max;
      data.az = sensorValue.un.linearAcceleration.z/a_max;
      break;
    // case SH2_GYROSCOPE_CALIBRATED:
    //   data.gx = sensorValue.un.gyroscope.x/g_max;
    //   data.gy = sensorValue.un.gyroscope.y/g_max;
    //   data.gz = sensorValue.un.gyroscope.z/g_max;
    //   break;
    case SH2_GRAVITY:
      data.ox = sensorValue.un.gravity.y/a_constant;
      data.oy = -sensorValue.un.gravity.x/a_constant;
      data.oz = sensorValue.un.gravity.z/a_constant;
      send = true;
      break;
  }
  //====================BNO-08X=======================


  if (send) {
    Serial.write((uint8_t *)&data, sizeof(data));

    //=====================DEBUG========================
    // Serial.print("AX: ");
    // print(data.ax);
    // Serial.print("    AY: ");
    // print(data.ay);
    // Serial.print("    AZ: ");
    // print(data.az);
    // Serial.print("    GX: ");
    // print(data.gx);
    // Serial.print("    GY: ");
    // print(data.gy);
    // Serial.print("    GZ: ");
    // print(data.gz);
    // Serial.print("    OX: ");
    // print(data.ox);
    // Serial.print("    OY: ");
    // print(data.oy);
    // Serial.print("    OZ: ");
    // print(data.oz);
    // Serial.println();
    //=====================DEBUG========================
    
    send = false;
  }
}


void print(float num) {
  if(num >= 0) {
    Serial.print(" ");
  }
  if(num*num <= 100) {
    Serial.print(" ");
  }
  Serial.print(num);
}