//====================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;
//====================BNO-08X=======================

//=====================OLED=========================
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <cstdint>
#include <cstring>
#include <string>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define X_DIV 2 //pixels per data point
#define X_SPACE 10 //spacer for 'x', 'y', and 'z' labels on screen
#define X_COUNT (SCREEN_WIDTH-X_SPACE)/X_DIV
#define Y_SHIFT SCREEN_HEIGHT/3 + SCREEN_HEIGHT/6
#define X_SHIFT X_SPACE

int8_t x, y, z = 0;
int8_t X[X_COUNT], Y[X_COUNT], Z[X_COUNT] = {};
//=====================OLED=========================


void setup(void) {
  Serial.begin(9600);
  while (!Serial) delay(10);

  //====================BNO-08X=======================
  Serial.println("Adafruit BNO08x test!");

  if (!bno08x.begin_I2C()) {
    Serial.println("Failed to find BNO08x chip");
    while (1) { delay(10); }
  }

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

  //=====================OLED=========================
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (1) { delay(10); }
  }
  //=====================OLED=========================
}



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

  switch (sensorValue.sensorId) {
    case SH2_LINEAR_ACCELERATION:
      x = sensorValue.un.linearAcceleration.x;
      y = sensorValue.un.linearAcceleration.y;
      z = sensorValue.un.linearAcceleration.z;

      std::memmove(X+1, X, (X_COUNT-1)*sizeof(int8_t));
      std::memmove(Y+1, Y, (X_COUNT-1)*sizeof(int8_t));
      std::memmove(Z+1, Z, (X_COUNT-1)*sizeof(int8_t));
      X[0] = x;
      Y[0] = y;
      Z[0] = z;
  //====================BNO-08X=======================

  //=====================OLED=========================
      display.clearDisplay();
      for (int i = 0; i < X_COUNT-1; ++i) { //Draw a line from point-to-point
        display.drawLine(i*X_DIV+X_SHIFT,  X[i] + 0*Y_SHIFT, (i+1)*X_DIV+X_SHIFT,  X[i+1] + 0*Y_SHIFT, SSD1306_WHITE);
        display.drawLine(i*X_DIV+X_SHIFT,  Y[i] + 1*Y_SHIFT, (i+1)*X_DIV+X_SHIFT,  Y[i+1] + 1*Y_SHIFT, SSD1306_WHITE);
        display.drawLine(i*X_DIV+X_SHIFT,  Z[i] + 2*Y_SHIFT, (i+1)*X_DIV+X_SHIFT,  Z[i+1] + 2*Y_SHIFT, SSD1306_WHITE);
      }
      //Add labels
      display.setTextSize(1);
      display.setTextColor(1);
      display.setCursor(0, 0*Y_SHIFT-SCREEN_HEIGHT/6);
      display.write('x');
      display.setCursor(0, 1*Y_SHIFT-SCREEN_HEIGHT/6);
      display.write('y');
      display.setCursor(0, 2*Y_SHIFT-SCREEN_HEIGHT/6);
      display.write('z');
      display.display();
      break;
  //=====================OLED=========================
  }
}