#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>
#include <Preferences.h>

// WiFi stuff
const char* ssid = "sirening";
const char* pwd = "alarm_11735";
int wifiStatus = 1;

// for ArduinoOSC
const char* host = "192.168.4.200";
const int settings_port = 54000;
const int state_port = 54001;

// Button Defs
bool buttonState1 = false;
AiEsp32RotaryEncoder encoderButton1 = AiEsp32RotaryEncoder(34, 35, 32, -1, 4);
//bool ampButtonState = false;
//AiEsp32RotaryEncoder ampEncoderButton = AiEsp32RotaryEncoder(33, 25, 26, -1, 4);
int menuSelect = 0;  //0 = freq, 1 = ports, 2 = ip, 3 = wifi

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

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

// Preferences
#define RW_MODE false
#define RO_MODE true
Preferences prefs;

HardwareSerial freqSerial(1);

void killAll() {
  updateFreq(0.00);
  OscWiFi.send(host, state_port, "/state", motorIndex, 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;
  encoderButton1.setEncoderValue(frequency * 10.0);
  freqSerial.print("\nV" + String(hzToRads(frequency)) + "\n");
  updateDisplayValue(String(frequency));
  Serial.println("Frequency: " + String(frequency));
}

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

void setupPrefs() {

  prefs.begin("prefs", RO_MODE);
  bool tpInit = prefs.isKey("nvsInit");

  if (tpInit == false) {
    prefs.end();
    prefs.begin("prefs", RW_MODE);

    prefs.putUInt("ports", ports);
    prefs.putUInt("motorIndex", motorIndex);
    prefs.putUInt("wifiStatus", wifiStatus);
    prefs.putBool("nvsInit", true);

    prefs.end();
    prefs.begin("prefs", RO_MODE);
  }

  ports = prefs.getUInt("ports");
  motorIndex = prefs.getUInt("motorIndex");
  wifiStatus = prefs.getUInt("wifiStatus");
  prefs.end();
}

void setupOSC() {
  //OscWiFi.publish(host, state_port, "/state", motorIndex, frequency, amplitude)->setFrameRate(10.f);
  //OscWiFi.publish(host, state_port, "/state", motorIndex, frequency)->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);
    }
  });
}

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

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

//This needs to be more sophisticated. Moving on if it disconnects...
void initWiFi() {

  IPAddress ip(192, 168, 4, 200 + motorIndex);
  IPAddress gateway(192, 168, 4, 1);
  IPAddress subnet(255, 255, 0, 0);

  int attempts = 0;
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, pwd);
  WiFi.config(ip, gateway, subnet);
  updateDisplayWiFi("Connecting to WiFi", "");
  //Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED && attempts < 5) {
    //Serial.print('.');
    attempts += 1;
    delay(1000);
  }

  if (WiFi.status() != WL_CONNECTED) {
    updateDisplayWiFi("Could not connect", "");
    //Serial.print("Could not Connect");
    delay(1000);
    //WiFi.disconnect(true);
    //WiFi.mode(WIFI_OFF);
  }
  Serial.println(WiFi.localIP());
}

void disconnectWiFi() {
  WiFi.disconnect(true);  // Disconnect and clear credentials
  //WiFi.mode(WIFI_OFF);    // Turn off WiFi radio
  delay(100);  // Small delay to ensure disconnection
  updateDisplayWiFi("DISCONNECTED", "connect");
}

void setup() {

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

  setupPrefs();

  setupEncoderButton(encoderButton1, "freq");

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

  if (wifiStatus == 1) {
    initWiFi();
    setupOSC();
  }

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

void encoderButtonUpdate(AiEsp32RotaryEncoder& eb, bool& buttonState) {
  if (eb.encoderChanged()) {
    oled.setCursor(12, 5);
    if (menuSelect == 0) {
      frequency = eb.readEncoder() / 10.0;
      freqSerial.print("\nV" + String(hzToRads(frequency)) + "\n");
      OscWiFi.send(host, state_port, "/state", motorIndex, frequency);
      updateDisplayValue(String(frequency));
      Serial.println("Frequency: " + String(frequency));
    }
    if (menuSelect == 1) {
      ports = eb.readEncoder();
      prefs.begin("prefs", RW_MODE);
      prefs.putUInt("ports", ports);
      prefs.end();
      updateDisplayValue(String(ports));
      Serial.println("No. of Ports: " + String(ports));
    }
    if (menuSelect == 2) {
      motorIndex = eb.readEncoder();
      prefs.begin("prefs", RW_MODE);
      prefs.putUInt("motorIndex", motorIndex);
      prefs.end();
      updateDisplayValue(String(motorIndex));
      Serial.println("Siren Index: " + String(motorIndex));
    }
    if (menuSelect == 3) {
      wifiStatus = eb.readEncoder();
      prefs.begin("prefs", RW_MODE);
      if (wifiStatus == 1) {
        initWiFi();
        setupOSC();
        prefs.putUInt("wifiStatus", wifiStatus);
        if (WiFi.status() != WL_CONNECTED) {
          eb.setEncoderValue(0);
        }
        if (WiFi.status() != WL_CONNECTED) {
          eb.setEncoderValue(0);
          prefs.putUInt("wifiStatus", 0);

          updateDisplayWiFi("DISCONNECTED", "connect");

          //WiFi.disconnect(true);
          //WiFi.mode(WIFI_OFF);
        } else {
          updateDisplayWiFi("disconnect", "CONNECTED");
        }
      } else {
        prefs.putUInt("wifiStatus", wifiStatus);
        disconnectWiFi();
        eb.setEncoderValue(0);
      }
      prefs.end();
    }
  }

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

  //button is up
  if (!eb.isEncoderButtonDown() && buttonState) {
    prefs.begin("prefs", RW_MODE);
    Serial.println(prefs.getUInt("motorIndex", motorIndex));
    prefs.end();
    if (frequency > 0) {
      killAll();
    } else {
      menuSelect = (menuSelect + 1) % 4;
      if (menuSelect == 0) {
        updateDisplayMenu("Frequency");
        eb.setBoundaries(0, 5000, false);
        eb.setAcceleration(1000);
        eb.setEncoderValue(frequency * 10.0);
        updateDisplayValue(String(frequency));
        Serial.println("Frequency: " + String(frequency));
      }
      if (menuSelect == 1) {
        updateDisplayMenu("No. of Ports");
        eb.setBoundaries(2, 12, false);
        eb.setAcceleration(1);
        eb.setEncoderValue(ports);
        updateDisplayValue(String(ports));
        Serial.println("No. of Ports: " + String(ports));
      }
      if (menuSelect == 2) {
        updateDisplayMenu("Siren Index");
        eb.setBoundaries(1, 8, false);
        eb.setAcceleration(1);
        eb.setEncoderValue(motorIndex);
        updateDisplayValue(String(motorIndex));
        Serial.println("Motor Index: " + String(motorIndex));
      }
      if (menuSelect == 3) {
        updateDisplayMenu("WiFi");
        eb.setBoundaries(0, 1, false);
        eb.setAcceleration(1);
        if (WiFi.status() != WL_CONNECTED) {
          eb.setEncoderValue(0);
          updateDisplayWiFi("DISCONNECTED", "connect");
        } else {
          eb.setEncoderValue(1);
          updateDisplayWiFi("disconnect", "CONNECTED");
        }
      }
    }
    buttonState = false;
  }
}

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

  OscWiFi.update();

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

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();
}

void updateDisplayWiFi(String string1, String string2) {
  oled.set1X();
  oled.setCursor(12, 5);
  oled.print(string1);
  oled.clearToEOL();
  oled.println();
  oled.clearToEOL();
  oled.println();
  oled.setCursor(12, 7);
  oled.print(string2);
  oled.clearToEOL();
  oled.println();
}

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