#if defined(ESP8266)
#include <ESP8266WiFi.h>
#include "MPU9250.h"
#else
#include <WiFi.h>
#endif
#include <WiFiUdp.h>
#include <OSCMessage.h>

char ssid[] = "cohabit";          // your network SSID (name)
char pass[] = "lewifidecohabit";  // your network password

int update_rate = 1000000;


WiFiUDP Udp;
// A UDP instance to let us send and receive packets over UDP
const IPAddress outIp(192, 168, 0, 210);  // remote IP of your computer
const unsigned int outPort = 16403;        // remote port to receive OSC
const unsigned int localPort = 16384;      // local port to listen for OSC packets (actually not used for sending)

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

  // Connect to WiFi network
  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, pass);

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

  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());

  Serial.println("Starting UDP");
  Udp.begin(localPort);
  Serial.print("Local port: ");
#ifdef ESP32
  Serial.println(localPort);
#else
  Serial.println(Udp.localPort());
#endif
}
void ledtoggle(OSCMessage &msg) {
  switch (msg.getInt(0)) {
    case 0:
      digitalWrite(LED_BUILTIN, LOW);
      break;
    case 1:
      digitalWrite(LED_BUILTIN, HIGH);
      break;
  }
}

void receiveMessage() {
  OSCMessage inmsg;
  int size = Udp.parsePacket();

  if (size > 0) {
    while (size--) {
      inmsg.fill(Udp.read());
    }
    if (!inmsg.hasError()) {
      inmsg.dispatch("/led", ledtoggle);
    }
    //else { auto error = inmsg.getError(); }
  }
}

void loop() {
  receiveMessage();
  OSCMessage msg("/step");
  msg.add(1);
  Udp.beginPacket(outIp, outPort);
  msg.send(Udp);
  Udp.endPacket();
  msg.empty();
  delayMicroseconds(update_rate);
  OSCMessage msg2("/Step");
  msg2.add(0);
  Udp.beginPacket(outIp, outPort);
  msg2.send(Udp);
  Udp.endPacket();
  msg2.empty();
  delayMicroseconds(update_rate);
  

  
}