#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>

// 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 ArduinoOSC
const char* hosts[] = {"192.168.4.201", "192.168.4.202", "192.168.4.203"};
const int settings_port = 54000;
const int state_port = 54001;

// Button Defs
bool freqButtonState = false;
AiEsp32RotaryEncoder freqEncoderButton = 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[] = {0, 0, 0};
float amplitudes[] = {0, 0, 0};
const int ports = 5;

// Display Defs
#define I2C_ADDRESS 0x3C
SSD1306AsciiWire oled;
uint8_t fieldWidth;
uint8_t rowsPerLine; // Rows per line.

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

float hzToRads(int freq){
  return freq * TWO_PI / ports;
}

float rpmToRads(int rpm){
  return (rpm / 60) * TWO_PI;
}

void setupOSC(char* path){
  OscWiFi.subscribe(state_port, path, [](const OscMessage& m) {
    int motorIndex =  m.arg<int>(0);
    float frequency = m.arg<float>(1);
    float amplitude = m.arg<float>(2);
    if(frequency != frequencies[motorIndex]){
      frequencies[motorIndex] = frequency;
      //freqEncoderButton.setEncoderValue(frequencies[sirenSelect] * 10.0);
      updateDisplay();
    }
    if(amplitude != amplitudes[motorIndex]){
      amplitudes[motorIndex] = amplitude;
      //ampEncoderButton.setEncoderValue(amplitudes[sirenSelect] * 10.0);
      updateDisplay();
    }
  });
}

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

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

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

void setup() {

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

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

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

  setupOSC("/state");

  setupEncoderButton(freqEncoderButton, "freq");
  setupEncoderButton(ampEncoderButton, "amp");

  Wire.begin();
  oled.begin(&Adafruit128x64, I2C_ADDRESS);
  //oled.setI2cClock(100000L);
  oled.setFont(System5x7);
  oled.clear();
  oled.println();
  oled.println();
  oled.println("  siren:     0");
  oled.println("  frequency: 0");
  oled.print("  amplitude: 0");
  fieldWidth = oled.fieldWidth(strlen("  amplitude: "));
  rowsPerLine = oled.fontRows();
}

void encoderButtonUpdate(AiEsp32RotaryEncoder& eb, bool& buttonState, char* val) {
  if (eb.encoderChanged()) {
    float setting = eb.readEncoder() / 10.0;
    OscWiFi.send(hosts[sirenSelect], settings_port, "/" + String(val), setting);
    Serial.print("new " + String(val) + " is: ");
    Serial.println(setting);
    updateDisplay();
  }

  if (eb.isEncoderButtonDown() && !buttonState) {
    buttonState = true;
  }

  //button is up
  if (!eb.isEncoderButtonDown() && buttonState) {
      if(val == "freq"){
        sirenSelect = (sirenSelect + 1) % 3;
        freqEncoderButton.setEncoderValue(frequencies[sirenSelect] * 10.0);
        ampEncoderButton.setEncoderValue(amplitudes[sirenSelect] * 10.0);
      } else {
        killAll();
      }
      updateDisplay();
      buttonState = false;
  }
}

void loop()
{
  encoderButtonUpdate(freqEncoderButton, freqButtonState, "freq");
  encoderButtonUpdate(ampEncoderButton, ampButtonState, "amp");

  OscWiFi.update();
}

void updateDisplay(){
  oled.clearField(fieldWidth, rowsPerLine * 2, 4);
  oled.print(sirenSelect + 1);
  oled.clearField(fieldWidth, rowsPerLine * 3, 4);
  oled.print(frequencies[sirenSelect]);
  oled.clearField(fieldWidth, rowsPerLine * 4, 4);
  oled.print(amplitudes[sirenSelect]);
}

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
    }
  }
}
*/
