#include "WiFi.h"

#include <Wire.h>

// Display Libraries
#include <Wire.h>
#include <SSD1306Ascii.h>
#include <SSD1306AsciiWire.h>

// Other Libraries
#include <arduino-timer.h>
#include <AiEsp32RotaryEncoder.h>
#include <ArduinoOSCWiFi.h>

const int sirenCount = 4;

// WiFi stuff
const char* ssid = "sirening";
const char* pwd = "alarm_11735";
const IPAddress ip(192, 168, 4, 200);
const IPAddress gateway(192, 168, 4, 1);
const IPAddress subnet(255, 255, 0, 0);

// for ArduinoOSCo
String hosts[sirenCount];
const int settings_port = 54000;
const int state_port = 54001;

// Button Defs
bool buttonState1 = false;
unsigned long startTimeButtonDown = 0;
unsigned int button1Count = 0;
AiEsp32RotaryEncoder encoderButton1 = AiEsp32RotaryEncoder(34, 35, 32, -1, 4);
//bool ampButtonState = false;
//AiEsp32RotaryEncoder ampEncoderButton = AiEsp32RotaryEncoder(33, 25, 26, -1, 4);
int sirenSelect = 0;

//TODO: These should be bound to a publication of the frequency directly from the siren
float frequencies[sirenCount] = {0};
//float frequencyTargets[] = {0, 0, 0};
//float amplitudes[] = {0, 0, 0};

// Display Defs
#define I2C_ADDRESS 0x3C
SSD1306AsciiWire oled;

void killAll() {
  for (int h = 0; h < sirenCount; h++) {
    OscWiFi.send(hosts[h], settings_port, "/kill");
    frequencies[h] = 0;
  }
}

void setupOSC(char* path) {
  OscWiFi.subscribe(state_port, path, [](const OscMessage& m) {
    int motorIndex = m.arg<int>(0) - 1;
    float frequency = m.arg<float>(1);
    if (frequency != frequencies[motorIndex]) {
      frequencies[motorIndex] = frequency;
      encoderButton1.setEncoderValue(frequency * 10);
      if (sirenSelect == motorIndex){
        updateDisplayValue(String(frequency));
      }
      Serial.println("Frequency: " + String(frequency));
    }
    /*
    if(frequency != frequencyTargets[motorIndex]){
      OscWiFi.send(hosts[sirenSelect], settings_port, "/freq", frequencyTargets[sirenSelect]);
    }
    */
  });
   OscWiFi.subscribe(state_port, "/freq", [](const OscMessage& m) {
    int siren =  m.arg<int>(0) - 1;
    float input = m.arg<float>(1);
    if (input < 500.0 && input >= 0.0) {
      frequencies[siren] = input;
      OscWiFi.send(hosts[siren], settings_port, "/freq", frequencies[siren]);
      Serial.println("Frequency: " + String(frequencies[siren]));

      encoderButton1.setEncoderValue(frequencies[sirenSelect] * 10.0);
      updateDisplayValue(String(frequencies[sirenSelect]));
    }
  });
}

void IRAM_ATTR readFreqEncoderISR() {
  encoderButton1.readEncoder_ISR();
}

/*
void IRAM_ATTR readAmpEncoderISR() {
  ampEncoderButton.readEncoder_ISR();
}
*/

void setupEncoderButton(AiEsp32RotaryEncoder& eb, char* val) {
  eb.begin();
  if (val == "freq") {
    eb.setup(readFreqEncoderISR);
    eb.setBoundaries(0, 5000, false);
    eb.setAcceleration(1000);
  }
}

void setup() {

  delay(2000);

  // initialize serial communication at 115200 bits per second:
  Serial.begin(115200);  //used for debugging.
  delay(2000);

  for (int i = 0; i < sirenCount; i++) {
    hosts[i] = "192.168.4." + String(200 + i + 1);
    Serial.println(hosts[i]);
  }

  WiFi.softAPConfig(ip, gateway, subnet);
  WiFi.softAP(ssid, pwd, 1, false, 5);

  /*
  Serial.println("");
  Serial.print("WiFi AP IP = ");
  Serial.println(WiFi.softAPIP());
  */

  setupOSC("/state");

  setupEncoderButton(encoderButton1, "freq");
  //setupEncoderButton(ampEncoderButton, "amp");

  Wire.begin();
  oled.begin(&Adafruit128x64, I2C_ADDRESS);
  //oled.setI2cClock(100000L);
  oled.setFont(System5x7);
  oled.clear();

  updateDisplayMenu("Frequency (Siren 1)");
  oled.setCursor(4, 3);
  oled.println("------------------");
  updateDisplayValue("0.00");
}

void encoderButtonUpdate(AiEsp32RotaryEncoder& eb, bool& buttonState, char* val) {

  if((millis() - startTimeButtonDown) > 500){
      button1Count = 0;
    }

  if (eb.encoderChanged()) {
    frequencies[sirenSelect] = eb.readEncoder() / 10.0;
    OscWiFi.send(hosts[sirenSelect], settings_port, "/" + String(val), frequencies[sirenSelect]);
    updateDisplayValue(String(frequencies[sirenSelect]));
    Serial.println("Frequency: " + String(frequencies[sirenSelect]));
  }

  if (eb.isEncoderButtonDown() && !buttonState) {
    buttonState = true;
    if(button1Count == 0) {
       startTimeButtonDown = millis();
    }
    button1Count = button1Count + 1;
    if(button1Count == 2 && (millis() - startTimeButtonDown <= 500)){
      killAll();
      sirenSelect = -1;//(sirenSelect - 2) % 3;
    }
  }

  //button is up
  if (!eb.isEncoderButtonDown() && buttonState) {
    if(val == "freq"){
      sirenSelect = (sirenSelect + 1) % sirenCount;
      updateDisplayMenu("Frequency (Siren " + String((sirenSelect+1)) + ")");
      encoderButton1.setEncoderValue(frequencies[sirenSelect] * 10.0);
      //ampEncoderButton.setEncoderValue(amplitudes[sirenSelect] * 10.0);
    } 
    updateDisplayValue(String(frequencies[sirenSelect]));
    Serial.println("Frequency: " + String(frequencies[sirenSelect]));
    buttonState = false;
  }
}

void loop() {
  encoderButtonUpdate(encoderButton1, buttonState1, "freq");
  //encoderButtonUpdate(ampEncoderButton, ampButtonState, "amp");

  OscWiFi.update();
}

void updateDisplayMenu(String string) {
  oled.set1X();
  oled.setCursor(4, 2);
  oled.print(string);
  oled.clearToEOL();
  oled.println();
}

void updateDisplayValue(String string) {
  oled.set2X();
  oled.setCursor(12, 5);
  oled.print(string);
  oled.clearToEOL();
  oled.println();
  oled.clearToEOL();
  oled.println();
}

bool showSerialData(void*) {

  Serial.print("\nrpm/freq real: ");
  Serial.print(frequencies[sirenSelect]);
  //Serial.print("\t");
  //Serial.print(frequencies[sirenSelect] / 60 * ports);

  return true;
}

/*
void getSerialData() {
  if (Serial.available() > 0) {
    // ignore line feed and carriage return
    if (Serial.peek() == '\n' || Serial.peek() == '\r') {
      Serial.read();  // read and discard LF & CR
    } else {
      frequency[sirenSelect] = Serial.parseFloat();
      Serial.println(frequency[sirenSelect], 4);  // print to 4 decimal places
    }
  }
}
*/
