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

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;

class Data {
public:
  float ax, ay, az;
};
Data data;
bool send;
//====================BNO-08X=======================


void setup(void) {
  Serial.begin(9600);
  //=====================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_LINEAR_ACCELERATION)) {
    Serial.println("Could not enable linear acceleration");
  }
  //====================BNO-08X=======================
}


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


  switch (sensorValue.sensorId) {
    case SH2_LINEAR_ACCELERATION:
      data.ax = sensorValue.un.linearAcceleration.x;
      data.ay = sensorValue.un.linearAcceleration.y;
      data.az = sensorValue.un.linearAcceleration.z;
      send = true;
      break;
  }
  //====================BNO-08X=======================


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

    //=====================DEBUG========================
    Serial.print("AX: ");
    print(data.ax);
    Serial.print("    AY: ");
    print(data.ay);
    Serial.print("    AZ: ");
    print(data.az);
    Serial.println();
    //=====================DEBUG========================
    send = false;
  }
}


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