//=====================Wi-Fi========================
#include <WiFi.h>
#include <WiFiClient.h>
#include <WiFiAP.h>
#include <WiFiUdp.h>
const char *ssid = "username";
const char *password = "password";
unsigned int port = 50000;

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


//====================BNO-08X=======================
class Data {
public:
  float ax, ay, az;
};
Data data;
//====================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() {
  Serial.begin(115200);

  //=====================Wi-Fi========================
  WiFi.mode(WIFI_AP);
  WiFi.softAP(ssid, password);
  IPAddress IP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(IP);

  udp.begin(port);
  Serial.printf("UDP server on port %d\n", port);
  //=====================Wi-Fi========================


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

  int packetSize = udp.parsePacket();
  if (packetSize) {
    udp.read((uint8_t *)&data, packetSize);

    //=====================OLED=========================
    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] = data.ax;
    Y[0] = data.ay;
    Z[0] = data.az;

    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.println();
    //=====================DEBUG========================
  }
}

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