#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";
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);
int menuSelect = 0; //0 = freq, 1 = ports

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

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

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

HardwareSerial freqSerial(1);

void killAll() {
  updateFreq(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 setupPrefs(){

   prefs.begin("STCPrefs", RO_MODE);           // Open our namespace (or create itif it doesn't exist) in RO mode.

   bool tpInit = prefs.isKey("nvsInit");       // Test for the existence of the "already initialized" key.

   if (tpInit == false) {
      // If tpInit is 'false', the key "nvsInit" does not yet exist therefore this must be our first-time run. We need to set up our Preferences namespace keys. So...
      prefs.end();                             // close the namespace in RO mode and...
      prefs.begin("prefs", RW_MODE);        //  reopen it in RW mode.


      // The .begin() method created the "STCPrefs" namespace and since this is our first-time run we will create our keys and store the initial "factory default" values.
      prefs.putUChar("ports", ports);
      //prefs.putString("talChan", "one");
      //prefs.putLong("talMax", -220226);
      //prefs.putBool("ctMde", true);

      prefs.putBool("nvsInit", true);          // Create the "already initialized"
                                                  //  key and store a value.

      // The "factory defaults" are created and stored so...
      prefs.end();                             // Close the namespace in RW mode and...
      prefs.begin("prefs", RO_MODE);        //  reopen it in RO mode so the setup code outside this first-time run 'if' block can retrieve the run-time values from the "STCPrefs" namespace.
   }

   // Retrieve the operational parameters from the namespace and save them into their run-time variables.
   ports = prefs.getUChar("ports");  //
   //tChannel = prefs.getString("talChan");            //  The LHS variables were defined
   //tChanMax = prefs.getLong("talMax");               //   earlier in the sketch.
   //ctMode = prefs.getBool("ctMde");                  //

   // All done. Last run state (or the factory default) is now restored.
   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() {
  freqEncoderButton.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() {
  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, 18, 19);
  delay(2000);

  initWiFi();

  setupOSC();

  setupEncoderButton(freqEncoderButton, "freq");

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

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

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

  //button is up
  if (!eb.isEncoderButtonDown() && buttonState) {
    if (frequency >= 0){
      killAll();
    } else {
      menuState = menuState + 1;
    }
    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 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
    }
  }
}
