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

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

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


//======================MIDI=========================
#include <Arduino.h>
#include <Adafruit_TinyUSB.h>
#include <MIDI.h>
Adafruit_USBD_MIDI usb_midi;
MIDI_CREATE_INSTANCE(Adafruit_USBD_MIDI, usb_midi, MIDI);
//======================MIDI=========================


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

  //======================MIDI=========================
  #if defined(ARDUINO_ARCH_MBED) && defined(ARDUINO_ARCH_RP2040)
  TinyUSB_Device_Init(0);   // Manual begin() is required on core without built-in support for TinyUSB such as mbed rp2040
  #endif
  pinMode(LED_BUILTIN, OUTPUT);
  MIDI.begin(MIDI_CHANNEL_OMNI);
  while( !TinyUSBDevice.mounted() ) delay(1);
  //======================MIDI=========================

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

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

void loop() {

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

    //======================MIDI=========================
    MIDI.sendControlChange(16, data.ax, data.channel);
    MIDI.sendControlChange(17, data.ay, data.channel);
    MIDI.sendControlChange(18, data.az, data.channel);
    MIDI.sendControlChange(19, data.a, data.channel);
    MIDI.sendControlChange(20, data.gx, data.channel);
    MIDI.sendControlChange(21, data.gy, data.channel);
    MIDI.sendControlChange(22, data.gz, data.channel);
    MIDI.sendControlChange(23, data.g, data.channel);
    MIDI.sendControlChange(24, data.ox, data.channel);
    MIDI.sendControlChange(25, data.oy, data.channel);
    MIDI.sendControlChange(26, data.oz, data.channel);
    MIDI.read();
    //======================MIDI=========================


    //=====================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========================
  }
}

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