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

const int C_FREEDFEED = 1;
const int C_FREEDHEAD = 2;
const int C_WRIGHTFEED = 3;
const int C_WRIGHTHEAD = 4;

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

int wrightHeadSpeed = 150;
int wrightFeedSpeed = 150;
int freedHeadSpeed = 150;
int freedFeedSpeed = 150;

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor* wrightHead = AFMS.getMotor(C_WRIGHTHEAD);
Adafruit_DCMotor* wrightFeed = AFMS.getMotor(C_WRIGHTFEED);
Adafruit_DCMotor* freedHead = AFMS.getMotor(C_FREEDHEAD);
Adafruit_DCMotor* freedFeed = AFMS.getMotor(C_FREEDFEED);

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

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("");
  Serial.println("WiFi connected.");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());
  Serial.print("ESP CPU Frequency in MHz: ");
  Serial.println(ESP.getCpuFreqMHz());

  wrightHead->setSpeed(wrightHeadSpeed);
  wrightHead->run(RELEASE);
  wrightFeed->setSpeed(wrightFeedSpeed);
  wrightFeed->run(RELEASE);
  wrightHead->setSpeed(freedHeadSpeed);
  freedHead->run(RELEASE);
  freedFeed->setSpeed(freedFeedSpeed);
  freedFeed->run(RELEASE);

  OscWiFi.subscribe(recv_port, "/stopall", [](const OscMessage& m) {
    wrightFeed->run(RELEASE);
    wrightHead->run(RELEASE);
    freedFeed->run(RELEASE);
    freedHead->run(RELEASE);
    
  });

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

    Serial.println(address);
    if (address == "/wright/feed/f") {
      wrightFeed->run(FORWARD);
    } else if (address == "/wright/feed/b") {
      wrightFeed->run(BACKWARD);
    } else if (address == "/wright/feed/r") {
      wrightFeed->run(RELEASE);
    } else if (address == "/wright/feed/speed") {
      wrightFeedSpeed = tidalSpeed;
      wrightFeed->setSpeed(wrightFeedSpeed);
    } else {
      Serial.println("dont know what to do with this OSC");
    };
  });

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

    Serial.println(address);
    if (address == "/wright/head/f") {
      wrightHead->run(FORWARD);
    } else if (address == "/wright/head/b") {
      wrightHead->run(BACKWARD);
    } else if (address == "/wright/head/r") {
      wrightHead->run(RELEASE);
    } else if (address == "/wright/head/speed") {
      wrightHeadSpeed = tidalSpeed;
      wrightHead->setSpeed(wrightHeadSpeed);
    } else {
      Serial.println("dont know what to do with this OSC");
    };
  });

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

    Serial.println(address);
    if (address == "/freed/feed/f") {
      freedFeed->run(FORWARD);
    } else if (address == "/freed/feed/b") {
      freedFeed->run(BACKWARD);
    } else if (address == "/freed/feed/r") {
      freedFeed->run(RELEASE);
    } else if (address == "/freed/feed/speed") {
      freedFeedSpeed = tidalSpeed;
      freedFeed->setSpeed(freedFeedSpeed);
    } else {
      Serial.println("dont know what to do with this OSC");
    };
  });

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

    Serial.println(address);
    if (address == "/freed/head/f") {
      freedHead->run(FORWARD);
    } else if (address == "/freed/head/b") {
      freedHead->run(BACKWARD);
    } else if (address == "/freed/head/r") {
      freedHead->run(RELEASE);
    } else if (address == "/freed/head/speed") {
      freedHeadSpeed = tidalSpeed;
      freedHead->setSpeed(freedHeadSpeed);
    } else {
      Serial.println("dont know what to do with this OSC");
    };
  });
  
}

void loop() {
  OscWiFi.update();
}