#include <Wire.h>
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <SPI.h>

const int MPU=0x68;
int16_t AcX,AcY,AcZ;

char ssid[] = "NETGEAR41";
char pass[] = "magicalbanana0";

WiFiUDP Udp;
const IPAddress outIp(192,168,0,29);   // remote IP of receiver computer
const unsigned int outPort = 7000;     // remote port to receive OSC


int fingerGround = 15;
int fingerSignal = 18;
int finger1 = 16;
int finger2 = 14;
int finger3 = 12;
int finger4 = 13;

void setup() {

  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.begin(115200);

  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());

  pinMode(fingerSignal, OUTPUT);
  pinMode(fingerGround, OUTPUT);
  digitalWrite(fingerSignal, HIGH);
  digitalWrite(fingerGround, LOW);

  pinMode(finger1, INPUT);
  pinMode(finger2, INPUT);
  pinMode(finger3, INPUT);
  pinMode(finger4, INPUT);

}

void loop() {

  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14);
  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();

  Serial.print(AcX);
  Serial.print("\t");
  Serial.print(AcY);
  Serial.print("\t");
  Serial.print(digitalRead(finger1));
  Serial.print("\t");
  Serial.print(digitalRead(finger2));
  Serial.print("\t");
  Serial.print(digitalRead(finger3));
  Serial.print("\t");
  Serial.print(digitalRead(finger4));
  Serial.println("\t");
  //Serial.println(AcZ);

  OSCMessage msgLX("/movedulation/sliderLX");
  msgLX.add(AcX);
  Udp.beginPacket(outIp, outPort);
  msgLX.send(Udp);
  Udp.endPacket();
  msgLX.empty();

  OSCMessage msgLY("/movedulation/sliderLY");
  msgLY.add(AcY);
  Udp.beginPacket(outIp, outPort);
  msgLY.send(Udp);
  Udp.endPacket();
  msgLY.empty();

  OSCMessage msgL1("/movedulation/toggleL1");
  msgL1.add(digitalRead(finger1));
  Udp.beginPacket(outIp, outPort);
  msgL1.send(Udp);
  Udp.endPacket();
  msgL1.empty();

  OSCMessage msgL2("/movedulation/toggleL2");
  msgL2.add(digitalRead(finger2));
  Udp.beginPacket(outIp, outPort);
  msgL2.send(Udp);
  Udp.endPacket();
  msgL2.empty();

  OSCMessage msgL3("/movedulation/toggleL3");
  msgL3.add(digitalRead(finger3));
  Udp.beginPacket(outIp, outPort);
  msgL3.send(Udp);
  Udp.endPacket();
  msgL3.empty();

  OSCMessage msgL4("/movedulation/toggleL4");
  msgL4.add(digitalRead(finger4));
  Udp.beginPacket(outIp, outPort);
  msgL4.send(Udp);
  Udp.endPacket();
  msgL4.empty();

  delay(100);
}