//=====================Wi-Fi========================
#include <WiFi.h>
#include <WiFiClient.h>
#include <WiFiAP.h>
#include <WiFiUdp.h>
const char *ssid = "laef";
const char *password = "laef4life";
const IPAddress serverIP(192, 168, 4, 1);
const int port = 1234;

WiFiUDP udp;
//=====================Wi-Fi========================


//====================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:
  uint8_t channel, ax, ay, az, a, gx, gy, gz, g, ox, oy, oz;
  //channel: sensor id
  //a: accelerometer
  //g: gyrometer
  //o: orientation
};
Data data;
bool send;
//====================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);
  data.channel = 1;

  //=====================Wi-Fi========================
  Serial.println();
  Serial.print("Connecting to WiFi");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("WiFi Connected");
  //=====================Wi-Fi========================


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

  Serial.println("BNO08x Found!");


  if (!bno08x.enableReport(SH2_ACCELEROMETER)) {
    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=======================


  //=====================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_ACCELEROMETER:
      data.ax = uint8_t((sensorValue.un.accelerometer.x + a_max) * 127/(2*a_max));
      data.ay = uint8_t((sensorValue.un.accelerometer.y + a_max) * 127/(2*a_max));
      data.az = uint8_t((sensorValue.un.accelerometer.z + a_max) * 127/(2*a_max));

      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] = sensorValue.un.accelerometer.x;
      Y[0] = sensorValue.un.accelerometer.y;
      Z[0] = sensorValue.un.accelerometer.z;
      send = true;
      break;
    // case SH2_GYROSCOPE_CALIBRATED:
    //   data.gx = uint8_t((sensorValue.un.gyroscope.x + g_max) * 127/(2*g_max));
    //   data.gy = uint8_t((sensorValue.un.gyroscope.y + g_max) * 127/(2*g_max));
    //   data.gz = uint8_t((sensorValue.un.gyroscope.z + g_max) * 127/(2*g_max));
    //   break;
    // case SH2_GRAVITY:
    //   data.ox = uint8_t((sensorValue.un.gravity.x + a_constant) * 127/(2*a_constant));
    //   data.oy = uint8_t((sensorValue.un.gravity.y + a_constant) * 127/(2*a_constant));
    //   data.oz = uint8_t((sensorValue.un.gravity.z + a_constant) * 127/(2*a_constant));
    //   send = true;
    //   break;
  }
  //====================BNO-08X=======================


  if (send) {
    //=====================Wi-Fi========================
    udp.beginPacket(serverIP, port);
    udp.write((uint8_t *)&data, sizeof(data));
    udp.endPacket();
    //=====================Wi-Fi========================


    //=====================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();
    //=====================OLED=========================


    //=====================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(uint8_t num) {
  if(num >= 0) {
    Serial.print(" ");
  }
  if(num*num <= 100) {
    Serial.print(" ");
  }
  Serial.print(num);
}