#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, 203);
const IPAddress gateway(192, 168, 4, 1);
const IPAddress subnet(255, 255, 0, 0);

// for ArduinoOSC
const char* host = "192.168.4.200";
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);

float frequency = 0;
float amplitude = 0;
const int ports = 6;
const int motorIndex = 2;

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

HardwareSerial freqSerial(1);
HardwareSerial ampSerial(2);

void killAll() {
  updateFreq(0.0);
  updateAmp(0.0);
}

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

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

void updateFreq(float val){
  frequency = val;
  Serial.println("new freq is: " + String(frequency));
  freqEncoderButton.setEncoderValue(frequency * 10.0);
  freqSerial.print("\nV" + String(hzToRads(frequency)) + "\n");
  updateDisplay();
}

void updateAmp(float val){
  amplitude = val;
  Serial.println("new amp is: " + String(amplitude));
  ampEncoderButton.setEncoderValue(amplitude * 10.0);
  ampSerial.print("\nV" + String(hzToRads(amplitude)) + "\n");
  updateDisplay();
}

void setupOSC(){
  OscWiFi.publish(host, state_port, "/state", motorIndex, frequency, amplitude)->setFrameRate(10.f);

  OscWiFi.subscribe(settings_port, "/kill", [](const OscMessage& m) {
    killAll();
  });

  OscWiFi.subscribe(settings_port, "/freq", [](const OscMessage& m) {
    float input = m.arg<float>(0);
    if(input < 500.0 && input >= 0.0){
      updateFreq(input);
    }
  });

  OscWiFi.subscribe(settings_port, "/amp", [](const OscMessage& m) {
    float input = m.arg<float>(0);
    if(input < 500.0 && input >= 0.0){
      updateAmp(input);
    }
  });
}

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, 5000, false);
  eb.setAcceleration(1000);
}

//This needs to be more sophisticated. Moving on if it disconnects...
void initWiFi() {
  int attempts = 0;
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, pwd);
  WiFi.config(ip, gateway, subnet);
  Serial.println();
  Serial.print("Connecting to WiFi ..");
  while (WiFi.status() != WL_CONNECTED && attempts < 5) {
    Serial.print('.');
    attempts += 1;
    delay(1000);
  }
  Serial.println(WiFi.localIP());
}

void setup() {

  Serial.begin(115200);
  freqSerial.begin(115200, SERIAL_8N1, 16, 17);
  ampSerial.begin(115200, SERIAL_8N1, 18, 19);
  delay(2000);

  initWiFi();

  setupOSC();

  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("  frequency: 0");
  oled.print("  amplitude: 0");
  fieldWidth = oled.fieldWidth(strlen("  amplitude: "));
  rowsPerLine = oled.fontRows();
}

void encoderButtonUpdate(char* val, float& setting, AiEsp32RotaryEncoder& eb, bool& buttonState, HardwareSerial& ser) {
  if (eb.encoderChanged()) {
    setting = eb.readEncoder() / 10.0;
    ser.print("\nV" + String(hzToRads(setting)) + "\n");
    Serial.println("new " + String(val) + " is: " + String(setting));
    updateDisplay();
  }

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

  //button is up
  if (!eb.isEncoderButtonDown() && buttonState) {
    killAll();
    buttonState = false;
  }
}

void loop()
{
  encoderButtonUpdate("freq", frequency, freqEncoderButton, freqButtonState, freqSerial);
  encoderButtonUpdate("amp", amplitude, ampEncoderButton, ampButtonState, ampSerial);

  OscWiFi.update();

  //getSerialData();
  getFreqSerialData();
  getAmpSerialData();

}

void updateDisplay(){
  oled.clearField(fieldWidth, rowsPerLine * 2, 4);
  oled.print(frequency);
  oled.clearField(fieldWidth, rowsPerLine * 3, 4);
  oled.print(amplitude);
}

bool showSerialData(void *) {

  Serial.print("\nrpm/freq real: ");
  Serial.print(frequency);
  Serial.print("\t");
  Serial.print(frequency / 60 * ports);

  return true;
}

void getFreqSerialData() {
  while (freqSerial.available()) {
    Serial.print(char(freqSerial.read()));
  }
}

void getAmpSerialData() {
  while (ampSerial.available()) {
    Serial.print(char(ampSerial.read()));
  }
}

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 = Serial.parseFloat();
      Serial.println(frequency, 4);  // print to 4 decimal places
    }
  }
}
