#include <ArduinoOSCWiFi.h>
#include <Adafruit_MotorShield.h>
#include <Button.h>


const int C_WHITEHEAD = 1;
const int C_WHITEFEED = 2;
const int C_BLACKFEED = 3;
const int C_BLACKHEAD = 4;


Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor* blackHead = AFMS.getMotor(C_BLACKHEAD);
Adafruit_DCMotor* blackFeed = AFMS.getMotor(C_BLACKFEED);
Adafruit_DCMotor* whiteFeed = AFMS.getMotor(C_WHITEFEED);
Adafruit_DCMotor* whiteHead = AFMS.getMotor(C_WHITEHEAD);

// customise as needed
const char* ssid = "XPUB";
const char* password = "5fff132d";
const char* host = "192.168.1.174";
const IPAddress ip(192, 168, 1, 171);
const IPAddress gateway(192, 168, 1, 1);
const IPAddress subnet(255, 255, 255, 0);

Button button1(D3);
Button button2(D7);
Button button3(D8);
Button button4(D4);
Button button5(D5);

const int recv_port = 10000;
const int send_port = 6060;

int blackHeadSpeed = 150;
int blackFeedSpeed = 150;
int whiteFeedSpeed = 150;
int whiteHeadSpeed = 150;

int STATE_SOLO = 0;

bool featureSolo = false;
bool featureB2 = false;
bool featureB4 = false;
bool featureB5 = false;
float b2Val = 1;
String b4Val = "chop";
float b5Val = 1;
String b4choices[2] = { "feed", "head" };

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

  // WiFi Stuff
  // WiFi.setMinSecurity(WIFI_AUTH_WPA_PSK); // didnt work on esp?
  // WiFi.mode(WIFI_MODE_STA); // didnt work on esp?
  WiFi.begin(ssid, password);
  WiFi.config(ip, gateway, subnet);

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

  if (!AFMS.begin()) { # here for pwm
    Serial.println("Could not find Motor Shield. Check wiring.");
    while (1)
      ;
  };

  Serial.println("Setting up the buttons");
  button1.begin();
  button2.begin();
  button3.begin();
  button4.begin();
  button5.begin();

  Serial.println("");
  Serial.println("WiFi connected.");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  Serial.print("ESP CPU Frequency in MHz: ");
  Serial.println(ESP.getCpuFreqMHz());


  whiteHead->setSpeed(whiteHeadSpeed);
  whiteHead->run(RELEASE);	
  blackHead->setSpeed(blackHeadSpeed);
  blackHead->run(RELEASE);
  whiteFeed->setSpeed(whiteFeedSpeed);
  whiteFeed->run(RELEASE);
  blackFeed->setSpeed(blackFeedSpeed);
  blackFeed->run(RELEASE);
	
  OscWiFi.subscribe(recv_port, "/stopall",
                    [](const OscMessage& m) {
                      blackFeed->run(RELEASE);
                      blackHead->run(RELEASE);
                      whiteFeed->run(RELEASE);
											whiteHead->run(RELEASE);
                    });

  OscWiFi.subscribe(recv_port, "/black/feed/*",
                    [](const OscMessage& m) {
                      float tidalSpeed = m.arg<float>(0);
                      String address = m.address();
                      // Serial.println(address);

                      if (STATE_SOLO == C_BLACKFEED || STATE_SOLO == 0) {
                        Serial.println(address);
                        if (address == "/black/feed/f") {
                          blackFeed->run(FORWARD);
                        } else if (address == "/black/feed/b") {
                          blackFeed->run(BACKWARD);
                        } else if (address == "/black/feed/r") {
                          blackFeed->run(RELEASE);
                        } else if (address == "/black/feed/speed") {
                          blackFeedSpeed = tidalSpeed;
                          blackFeed->setSpeed(blackFeedSpeed);
                        } else {
                          Serial.println("dont know what to do with this OSC");
                        };
                      } else {
                        Serial.println("dont mind me, feeds' not part of this solo");
                      };
                    });

  OscWiFi.subscribe(recv_port, "/white/feed/*",
                    [](const OscMessage& m) {
                      float tidalSpeed = m.arg<float>(0);
                      String address = m.address();
                      // Serial.println(address);

                      if (STATE_SOLO == C_WHITEFEED || STATE_SOLO == 0) {
                        Serial.println(address);
                        if (address == "/white/feed/f") {
                          whiteFeed->run(FORWARD);
                        } else if (address == "/white/feed/b") {
                          whiteFeed->run(BACKWARD);
                        } else if (address == "/white/feed/r") {
                          whiteFeed->run(RELEASE);
                        } else if (address == "/black/feed/speed") {
                          whiteFeedSpeed = tidalSpeed;
                          whiteFeed->setSpeed(whiteFeedSpeed);
                        } else {
                          Serial.println("dont know what to do with this OSC");
                        };
                      } else {
                        Serial.println("dont mind me, feeds' not part of this solo");
                      };
                    });
		
  OscWiFi.subscribe(recv_port, "/black/head/*",
                    [](const OscMessage& m) {
                      float tidalSpeed = m.arg<float>(0);
                      String address = m.address();
                      // Serial.println(address);

                      if (STATE_SOLO == C_BLACKHEAD || STATE_SOLO == 0) {
                        Serial.println(address);
                        if (address == "/black/head/f") {
                          blackHead->run(FORWARD);
                        } else if (address == "/black/head/b") {
                          blackHead->run(BACKWARD);
                        } else if (address == "/black/head/r") {
                          blackHead->run(RELEASE);
                        } else if (address == "/black/head/speed") {
                          blackHeadSpeed = tidalSpeed;
                          blackHead->setSpeed(blackHeadSpeed);
                        } else {
                          Serial.println("dont know what to do with this OSC");
                        };
                      } else {
                        Serial.println("dont mind me, heads' not part of this solo");
                      };
                    });

	  OscWiFi.subscribe(recv_port, "/white/head/*",
                    [](const OscMessage& m) {
                      float tidalSpeed = m.arg<float>(0);
                      String address = m.address();
                      // Serial.println(address);

                      if (STATE_SOLO == C_WHITEHEAD || STATE_SOLO == 0) {
                        Serial.println(address);
                        if (address == "/white/head/f") {
                          whiteHead->run(FORWARD);
                        } else if (address == "/white/head/b") {
                          whiteHead->run(BACKWARD);
                        } else if (address == "/white/head/r") {
                          whiteHead->run(RELEASE);
                        } else if (address == "/white/head/speed") {
                          whiteHeadSpeed = tidalSpeed;
                          whiteHead->setSpeed(whiteHeadSpeed);
                        } else {
                          Serial.println("dont know what to do with this OSC");
                        };
                      } else {
                        Serial.println("dont mind me, heads' not part of this solo");
                      };
                    });

		/* OscWiFi.subscribe(recv_port, "/scanner/\*", */
  /*                   [](const OscMessage& m) { */
  /*                     float tidalSpeed = m.arg<float>(0); */
  /*                     String address = m.address(); */
  /*                     // Serial.println(address); */

  /*                     if (STATE_SOLO == C_WHITEFEED || STATE_SOLO == 0) { */
  /*                       Serial.println(address); */
  /*                       if (address == "/scanner/f") { */
  /*                         whiteFeed->run(FORWARD); */
  /*                       } else if (address == "/scanner/b") { */
  /*                         whiteFeed->run(BACKWARD); */
  /*                       } else if (address == "/scanner/~") { */
  /*                         whiteFeed->run(RELEASE); */
  /*                       } else if (address == "/scanner/speed") { */
  /*                         whiteFeedSpeed = tidalSpeed; */
  /*                         whiteFeed->setSpeed(whiteFeedSpeed); */
  /*                       } else { */
  /*                         Serial.println("dont know what to do with this OSC"); */
  /*                       }; */
  /*                     } else { */
  /*                       Serial.println("dont mind me, scanners not part of this solo"); */
  /*                     }; */
  /*                   }); */

}



void loop() {
  OscWiFi.update();

  bool b1p = button1.pressed();
  bool b2p = button2.pressed();
  bool b4p = button4.pressed();
  bool b5p = button5.pressed();

  bool b1 = button1.read();
  bool b2 = button2.read();
  bool b4 = button4.read();
  bool b5 = button5.read();
  // Serial.print("b4p");
  // Serial.println(b4p);

  // Give dibs to button 1
  if (b1p && b2 != Button::PRESSED && b4 != Button::PRESSED && b5 != Button::PRESSED) {
    Serial.println("solo has dibs");
    featureSolo = true;
  }
  if (button1.released()) {
    Serial.println("solo finished dibs");
    featureSolo = false;

    // handle ending of feature solo;
    if (STATE_SOLO != 0) {
      Serial.println("stop the solo");
      STATE_SOLO = 0;
      blackFeed->run(blackFeedSpeed);
      blackHead->run(blackHeadSpeed);
      whiteFeed->run(whiteFeedSpeed);
			whiteHead->run(whiteHeadSpeed);
    }
  }

  // give dibs to button 2
  if (b2p && b1 != Button::PRESSED && b4 != Button::PRESSED && b5 != Button::PRESSED) {
    Serial.println("B2 has dibs");
    featureB2 = true;
  }
  if (button2.released()) {
    Serial.println("B2 finished dibs");

    // handle ending of feature B2;
    if (featureB2) {
      Serial.print("did many calculations and now sending the value of ");
      Serial.println(b2Val);
      OscWiFi.send(host, send_port, "/ctrl", "b2Val", b2Val);
      b2Val = 1;
      featureB2 = false;
    }
  }

  // give dibs to button 4 (and do the thing rightaway);
  if (b4p && b1 != Button::PRESSED && b2 != Button::PRESSED && b5 != Button::PRESSED) {
    Serial.println("button4 has dibs");
    featureB4 = true;
    
    int index = random(9);
    b4Val = b4choices[index];
    Serial.print("sending feature b4: ");
    Serial.println(b4Val);
  }
  if (button4.released()) {
    Serial.println("button4 finished dibs");
    if (featureB4) {
      featureB4 = false;
      b4Val = "feed";
    }
  }

  // feature: SOLO!
  if (b1 == Button::PRESSED && featureSolo) {
    if (b2p) {
      Serial.println("solo feed");
      STATE_SOLO = C_BLACKFEED;
      blackHead->run(RELEASE);
      blackHead->setSpeed(0);
      whiteHead->run(RELEASE);
      whiteHead->setSpeed(0);
    }

    if (b4p) {
      Serial.println("solo head");
      STATE_SOLO = C_BLACKHEAD;
      blackFeed->run(RELEASE);
      blackFeed->setSpeed(0);
      whiteFeed->run(RELEASE);
      whiteFeed->setSpeed(0);
    }

    /* if (b5p) { */
    /*   Serial.println("solo scanner"); */
    /*   STATE_SOLO = C_WHITEFEED; */
    /*   blackHead->run(RELEASE); */
    /*   blackFeed->run(RELEASE); */
    /*   blackHead->setSpeed(0); */
    /*   blackFeed->setSpeed(0); */
    /* } */
  }

  // feature: Send B2 OSC
  if (b2 == Button::PRESSED && featureB2) {
    if (b1p) {
      float newVal = b2Val * .5;
      b2Val = newVal;
      Serial.print("2 + 1: ");
      Serial.println(b2Val);
    }

    if (b4p) {
      float newVal = b2Val * 2;
      b2Val = newVal;
      Serial.print("2 + 4: ");
      Serial.println(b2Val);
    }

    if (b5p) {
      float newVal = b2Val * 4;
      b2Val = newVal;
      Serial.print("2 + 5: ");
      Serial.println(b2Val);
    }
  }

  // feature: Send a string
  if (b4 == Button::PRESSED && featureB4) {
    if (b1p) {
      Serial.println("sending the string via button 1");
      OscWiFi.send(host, send_port, "/ctrl", "featureB41", b4Val);
    }

    if (b2p) {
      Serial.println("sending the string via button 1");
      OscWiFi.send(host, send_port, "/ctrl", "featureB42", b4Val);
    }

    /* if (b5p) { */
    /*   Serial.println("sending the string via button 1"); */
    /*   OscWiFi.send(host, send_port, "/ctrl", "featureB43", b4Val); */
    /* } */
  }
}