#include "Arduino.h"
#include "WiFi.h"
#include <OSCMessage.h>

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

int ledPin = D9;    //Define LED pin 

WiFiUDP Udp; // A UDP instance to let us send and receive packets over UDP
const IPAddress outIp(123,456,78,910); // IP to Send To

// Options
int update_rate = 16;

// Network settings
char ssid[] = "YOUR_WIFI_NAME"; // your network SSID (name)
char pass[] = "YOUR_PASSWORD";  // your network password
unsigned int localPort = 7776; // local port to listen for OSC packets
unsigned int sendPort = 7771; // Port to send to Max 

bool is_streaming = false;

Adafruit_MPU6050 mpu;

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

  pinMode(ledPin, OUTPUT);// Set ledPin as output mode 

   /* setup wifi */
  WiFi.begin(ssid, pass);
  Serial.println("\nConnecting");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(500);
  }

    Serial.println("\nConnected to the WiFi network");
    get_network_info();
 
  Udp.begin(localPort);

  // set accelerometer range to +-8G
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);

  // set gyro range to +- 500 deg/s
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);

  // set filter bandwidth to 21 Hz
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  delay(100);
}

void loop() {

      if(WiFi.status() == WL_CONNECTED){
      LED_On();
    }

      if(is_streaming){
      sendOSC();
    }

       receiveMessage();
    delay(10);

}

void LED_On(){
  digitalWrite(ledPin, HIGH);
}

void get_network_info(){
    if(WiFi.status() == WL_CONNECTED) {
        Serial.print("[*] Network information for ");
        Serial.println(ssid);

        Serial.println("[+] BSSID : " + WiFi.BSSIDstr());
        Serial.print("[+] Gateway IP : ");
        Serial.println(WiFi.gatewayIP());
        Serial.print("[+] Subnet Mask : ");
        Serial.println(WiFi.subnetMask());
        Serial.println((String)"[+] RSSI : " + WiFi.RSSI() + " dB");
        Serial.print("[+] ESP32 IP : ");
        Serial.println(WiFi.localIP());
    }
}

void sendOSC() {
  OSCMessage msg("/datafromesp");
 
 /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  msg.add(a.acceleration.x);
  msg.add(a.acceleration.y);
  msg.add(a.acceleration.z);

  msg.add(g.gyro.x);
  msg.add(g.gyro.y);
  msg.add(g.gyro.z);

  msg.add(temp.temperature);

  delay(100); 

  Udp.beginPacket(outIp, sendPort);
  msg.send(Udp); // Send the bytes to the SLIP stream
  Udp.endPacket();  // Mark the end of the OSC Packet
  msg.empty();   // Free space occupied by message
  delay(10);
}

void receiveMessage() {

  OSCMessage inmsg;
  int size = Udp.parsePacket();
 
  if (size > 0) {
    while (size--) {
      inmsg.fill(Udp.read());
        Serial.println("Received Message");

    }
    if (!inmsg.hasError()) {
      inmsg.dispatch("/toggle_stream", stream_toggle);
    } 

   
    //else { auto error = inmsg.getError(); }
  }
}

void stream_toggle(OSCMessage &msg) {
  switch (msg.getInt(0)) {
  case 0:
   is_streaming=false;
   Serial.println(is_streaming);
    break;
  case 1:
    is_streaming=true;
    Serial.println(is_streaming);
    break;
  }
}