//Travis Edrington
//This is arduino code for blinking an RGB strip at a rate matching song for a musical so
//  deaf artists can follow the beat.
//  This is for ESP32 devices.  Commands will be recived via UDP as OSC commands.
//  Example OSC commands.  They all start with /beat/
//  /beat/BPM/120 for Flashing colors at 120 Beats per minutes.
//  /beat/Color/800 for a Solid Red at 120 BPM.
//  /beat/BPM@M/120/20 to switch to 120 Beats at measure 20.
//  /beat/BLINK/120/008 For a single color blink at 120 BPM.
//  /beat/PAUSE to Pause
//  /beat/RESUME to Resume
//  /beat/TOGGLEPP to pause/resume
//  /beat/HALT to stop and reset
//  /Beat/heart/080 to start the heartbeat party
//  /beat/? to print to serial all variables.


#include <WiFi.h>
#include <WiFiMulti.h>
#include "AsyncUDP.h"
#include <EEPROM.h>
#include <LiquidCrystal_I2C.h>


// DEBUGging switches and macros
#define DEBUG 4    // Switch DEBUG output on and off by 1+ or 0
#define DEBUGS 1    // Switch Spamming DEBUG output on and off by 1+ or 0
#define SERIALAVAL 1 // Send to Serial if True //TODO
#define WIFI true    //Set to 0 if you want to avoid the wifi check
#define location 0  //Set for where to send Print commands.  0 for default of Serial.
#define wLine true

//---- Print handeling


  char s [60];
  #define PRINTOUT(s){Serial.print(s) ;}
  #define PRINTOUTN(s){Serial.println(s);}

  #define PRINTS(s) \
    { \
      Serial.print(s); \
      Serial.print(" @ Line "); \
      Serial.print(__LINE__); \
      Serial.println(); \
    }

  #define PRINT(s, v) \
    { \
      Serial.print(F(s)); \
      Serial.print(": "); \
      Serial.print(v); \
      Serial.print(" @ Line "); \
      Serial.print(__LINE__); \
      Serial.println(); \
    }

  #define PRINTV(v) \
    { \
      PRINTOUT(F(#v)); \
      PRINTOUT(": "); \
      PRINTOUTN(v); \
    }

  #define PRINTB() \
    { \
      PRINTOUTN("\n"); \
    }

//---- Color Setup
// Red, green, and blue pins for PWM control
const uint8_t redPin = 27;
const uint8_t greenPin = 32;
const uint8_t bluePin = 33;
const bool ledFlip = true;    // When direct driving an LED strip with common Anode, LED needs to be flipped.
bool FlipFlop = false;

const uint8_t statR = 0; //Incase I want to use status leds
const uint8_t statG = 0;
const uint8_t statB = 0;
const uint8_t statY = 0;

TaskHandle_t TaskSerial;
TaskHandle_t TaskLCD;

//---- Set up LCD
  // set the LCD number of columns and rows
  int lcdColumns = 16;
  int lcdRows = 2;
  // if you don't know your display address, run an I2C scanner sketch
  // LCD is connected SDA to D21, SDL to D22
  LiquidCrystal_I2C lcd(0x27, lcdColumns, lcdRows);
  uint8_t lcdMode = 0;  //mode 0 = network info, mode 1 = measure, mode 2 = other...for now
  bool lcdClear = true;
  unsigned long lastbuttontime = 0;
  uint8_t lcdButtonPin = 18;

  String networkS1 = "Not Connected";
  String networkS2 = "unknown";
  String networkS3 = "unknown";
  String networkS4 = "unknown";
  String lastStatus = "null";
  String sBlank = "               ";

  void lcdPrint1(String txt = ""){
    lcd.setCursor(0, 0);
    lcd.print(txt);
  }
  void lcdPrint2(String txt = ""){
    lcd.setCursor(0, 1);
    lcd.print(txt);
  }

  void IRAM_ATTR Button_isr() {
    if (esp_timer_get_time() - lastbuttontime > 1000){
      lcdMode = (lcdMode + 1) % 4;
      lcdClear = true;
    }
  }

//---- Beat Variable Setup
  //Time related commands
  unsigned long interval = 1000;

  unsigned long previousMicroS;  // will store last time LED was updated
  unsigned long currentMicroS;   // will store the current time
  unsigned long nextEvent;

  unsigned long previousMicroSPause;  // will store last time LED was updated
  unsigned long currentMicroSPause;   // will store the current time
  unsigned long nextEventPause;
  uint8_t pauseMode;
  

  const uint8_t PIN_BEATS = 2;  // Pin for outputting just a beat status light //Verify

  uint8_t beat = 0;  //0-15, Even is top of beat, odd for lights off
  // Modes:  Even = Active mode, Even+1 = Newly set mode
  // 0: Doing Nothing
  // 2: color blink
  // 4: Single Color Blink
  // 6: preset beat blink
  // 8: solid color
  //10: heartbeat
  //12: Pause
  uint8_t mode = 0;
  uint8_t lastmode = 0;  //for preventing spamming to serial
  uint8_t premeasure = 0;
  uint16_t measure = 1;
  uint16_t nextmeasure = 1;
  float nextBPM = 30.0;

  uint8_t DesiredLEDr = 255, DesiredLEDg = 0,DesiredLEDb = 0;

  unsigned long nextPulse = 0;  //For heartbeat timmer

  //Wakeup and standby heartbeat
  uint8_t brightness = 0;
  uint8_t heartFade = 5;
  const uint16_t heartRate = 25000;  //50 ms * (255/5 = 51) 500ms for a 1 sec cycle

  float metronomePWM = .5;
  float metronomeTotal = 1000;  // Some of how long each beat is on
  float metronomeON = 100;      // = 60000 / playBPM * .25; //How many MicroSeconds the light is on
  float metronomeOFF = 900;     // = 60000 / playBPM * .75; //How many MicroSeconds between each beat

//---- Other variables
  const char compile_date[] = __DATE__ " " __TIME__;
  uint8_t counter = 0;          // For those days you need a counter



//---- Wifi setup

  const unsigned long udpPort = 1234;
  WiFiMulti wifiMulti;
  AsyncUDP udp;

#if SERIALAVAL
  void get_network_info() {
    if (WiFi.status() == WL_CONNECTED) {
      PRINT("[*] Network information for", String(WiFi.SSID()));
      PRINT("[+] ESP32 IP",WiFi.localIP().toString());
      PRINT("[+] Gateway IP", WiFi.gatewayIP().toString());
      PRINT("[+] Subnet Mask",WiFi.subnetMask().toString());
      PRINT("[+] RSSI",WiFi.RSSI());
    }
  }
#endif

class RGB {
  // From https://zzzcode.ai/code-generator?id=7783a1a1-d6be-4b17-b33e-950498b5b615, https://zzzcode.ai/code-generator?id=9c9c9d2c-33f9-4815-94c7-ba9e3cf29a2e
  private:
    uint8_t redPin;
    uint8_t greenPin;
    uint8_t bluePin;
    uint8_t rainbowID = 0;
  public:
    // Constructor to initialize the RGB LED pins
    RGB(uint8_t rPin, uint8_t gPin, uint8_t bPin) {
      redPin = rPin;
      greenPin = gPin;
      bluePin = bPin;
      pinMode(redPin, OUTPUT);
      pinMode(greenPin, OUTPUT);
      pinMode(bluePin, OUTPUT);
      
    }

    // Method to set the color of the RGB LED
    void setColor(uint8_t redValue, uint8_t greenValue, int blueValue) {
      if (ledFlip) {
        analogWrite(redPin, 255 - redValue);
        analogWrite(greenPin, 255 - greenValue);
        analogWrite(bluePin, 255 - blueValue);
      } else {
        analogWrite(redPin, redValue);
        analogWrite(greenPin, greenValue);
        analogWrite(bluePin, blueValue);
      }
      #if DEBUGS >= 2
        PRINT("New Color" );
        PRINT("R", redValue);
        PRINT("G", greenValue);
        PRINT("B", blueValue);
      #endif

    }

    // Method to turn off the RGB LED
    void off() {
      setColor(0, 0, 0);
      #if DEBUGS >= 2
        PRINTS("[C] New Color: Off");
      #endif
    }

    void R() {  //Red
      setColor(255, 0, 0);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Red");
      #endif
    }

    void G() {  //Green
      setColor(0, 255, 0);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Green");
      #endif
    }

    void B() {  //Blue
      setColor(0, 0, 255);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Blue");
      #endif
    }

    void Y() {  //Yellow
      setColor(255, 255, 0);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Yellow");
      #endif
    }

    void C() {  //Cyan
      setColor(0, 255, 255);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Cyan");
      #endif
    }

    void M() {  //Magenta
      setColor(255, 0, 255);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Magenta");
      #endif
    }

    void O() {  //Orange
      setColor(255, 127, 0);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Orange");
      #endif
    }

    void P() {  //Purple
      setColor(127, 0, 255);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: Purple");
      #endif
    }

    void W() {  //White
      setColor(255, 255, 255);
      #if DEBUGS >= 3
        PRINTS("[C] New Color: White");
      #endif
    }


    void RainbowCycle() {
      //Cycle between 8 colors
      switch (rainbowID) {
        case 0:
          {
            R();
            break;
          }
        case 1:
          {
            O();
            break;
          }
        case 2:
          {
            Y();
            break;
          }
        case 3:
          {
            G();
            break;
          }
        case 4:
          {
            C();
            break;
          }
        case 5:
          {
            B();
            break;
          }
        case 6:
          {
            M();
            break;
          }
        case 7:
          {
            P();
            break;
          }
        default:
          {
            W();
            break;
          }
      }
      rainbowID = (rainbowID + 1) % 8;
    }
    void RGBCycle() {
      //Cycle between 3 colors
      switch (rainbowID) {
        case 0:
          {
            R();
            break;
          }
        case 1:
          {
            G();
            break;
          }
        case 2:
          {
            B();
            break;
          }
        default:
          {
            W();
            break;
          }
      }
      rainbowID = (rainbowID + 1) % 3;
    }
};



RGB DisplayLED(redPin, greenPin, bluePin);  // Initialize RGB LED


void varDump(){
    #if SERIALAVAL
    PRINTS("<---- START Global Variable Dump ---->");
    PRINTS("[*] WiFi connected. UDP Listening on IP:" + WiFi.localIP().toString() + ":" + String(udpPort) + " on " + String(WiFi.SSID()));
    PRINTB();
    PRINTV(networkS1);
    PRINTV(networkS2);
    PRINTV(networkS3);
    PRINTV(networkS4);
    PRINTV(lastStatus);
    PRINTV(lcdMode);
    PRINTS("----------------------------------------");
    
    PRINTV(interval);

    PRINTV(previousMicroS);
    PRINTV(currentMicroS);
    PRINTV(nextEvent);
    
    PRINTS("----------------------------------------");

    PRINTV(previousMicroSPause);
    PRINTV(currentMicroSPause);
    PRINTV(nextEventPause);
    PRINTV(pauseMode);

    PRINTS("----------------------------------------");
    PRINTV(beat);

    PRINTS("----------------------------------------");
    PRINTV(mode);
    PRINTV(modeName());
    PRINTV(lastmode);

    PRINTS("----------------------------------------");
    PRINTV(premeasure);
    PRINTV(measure);

    PRINTV(nextmeasure);
    PRINTV(nextBPM);

    PRINTS("----------------------------------------");
    PRINTV(DesiredLEDr);
    PRINTV(DesiredLEDg);
    PRINTV(DesiredLEDb);

    PRINTS("----------------------------------------");
    PRINTV(nextPulse);

    PRINTV(brightness);
    PRINTV(heartFade);
    PRINTV(heartRate);

    PRINTS("----------------------------------------");
    PRINTV(DesiredLEDg);
    PRINTV(metronomePWM);
    PRINTV(metronomeTotal);
    PRINTV(metronomeON);
    PRINTV(metronomeOFF);

    PRINTV(counter);
    PRINTB();

    PRINT("High Serial Total:", uxTaskGetStackHighWaterMark(TaskSerial));
    PRINT("High LCD Total:", uxTaskGetStackHighWaterMark(TaskLCD));
    PRINT("High Water Total:", uxTaskGetStackHighWaterMark(NULL));

    PRINTS("----------------------------------------");
    PRINTS("  /beat/BPM/120 for Flashing colors at 120 Beats per minutes.");
    PRINTS("  /beat/Color/800 for a Solid Red at 120 BPM.");
    PRINTS("  /beat/BPM@M/120/20 to switch to 120 Beats at measure 20.");
    PRINTS("  /beat/BLINK/120/008 For a single color blink at 120 BPM.");
    PRINTS("  /beat/PAUSE to Pause");
    PRINTS("  /beat/RESUME to Resume");
    PRINTS("  /beat/TOGGLEPP to pause/resume");
    PRINTS("  /beat/HALT to stop and reset");
    PRINTS("  /Beat/heart/080 to start the heartbeat party");
    
    PRINTS("<---- END Global Variable Dump ---->\n\n");
  #endif
}

uint8_t octToByte(uint8_t octVal = 0) { // Function to convert an octal value to a byte.
  #if DEBUG >= 2 // Check if DEBUGging level is set to 2 or higher
    PRINT("octVal: ", octVal); // Print the octal value for DEBUGging purposes
  #endif
  //TODO just convert to int before multiplying
  if (octVal - 48 >= 8) { // Check if the octal value is greater than or equal to 8
    return 255; // Return 255 if the value is out of valid octal range
  } else {
    return (octVal - 48) * 32; // Convert the octal value to byte and return it
  }
}

uint8_t hextToByte(char hexVal = '0') { //  Todo
  return 255;
}

String modeName(){
  switch (mode){
    case 0:
      return "Stopped";
      break;
    case 1:
      return "Stopped New";  //Triggered
      break;
    case 2:
      return "Regular BPM";
      break;
    case 3:
      return "Regular BPM New";   //Triggered
      break;
    case 4:
      return "Single Color Blink";
      break;
    case 5:
      return "Single Color Blink New";    //Triggered
      break;
    case 6:
      return "Preset Blink";
      break;
    case 7:
      return "Preset Blink New";    //I dont know why I have this
      break;
    case 8:
      return "Solid Color";
      break;
    case 9:
      return "Solid Color New";   //Triggered
      break;
    case 10:
      return "Heart Beat";
      break;
    case 11:
      return "Heart Beat New";  //Triggered
      break;
    case 12:
      return "Pause";   //Trigger
      break;
    case 13:
      return "Waiting for BPM Change";   //Trigger
      break;
    default:
      return "This should not happen";
      break;
  }
} 

void modePause(){
  if (mode != 12){ // Start Pause mode
    previousMicroSPause = previousMicroS;  // will store last time LED was updated
    currentMicroSPause = currentMicroS;   // will store the current time
    nextEventPause = nextEvent;
    pauseMode = mode;
    mode = 12;
  }
}
void modeResume(){
  if (mode == 12){ //resume last mode
    previousMicroS = previousMicroSPause;  // will store last time LED was updated
    currentMicroS = currentMicroSPause;   // will store the current time
    nextEvent = nextEventPause;
    mode = pauseMode;
  }
}

void BPMMath(float rawBPM){
  metronomeTotal = 60000000 / rawBPM;
  metronomeON = metronomeTotal * metronomePWM;
  metronomeOFF = metronomeTotal - metronomeON;
  
  #if DEBUG >= 1
    PRINT("BPM",rawBPM);
    PRINT("met Total", metronomeTotal);
    PRINT("met   OFF", metronomeOFF);
    PRINT("met    ON", metronomeON);
    PRINT("curr Time", currentMicroS);
  #endif
}
#if DEBUG >= 3
  void printtimes(){
    PRINTS("[!] Debug 3 or higher is on.")
    PRINT("[+] Last update time", previousMicroS);  // will store last time LED was updated
    PRINT("[+] Current Time    " , currentMicroS);   // will store the current time
    PRINT("[+] Next Time       " , nextEvent);
  }
#endif

//---- OSC setup
  class OSCParser {
    private:
      String command; // The OSC command string
      String components[4]; // Array to hold components (max 10 components)
      uint8_t parseLength;
      int componentCount; // Number of components parsed

    public:
      // Constructor to initialize the command
      OSCParser(String cmd, uint8_t len) {
        command = cmd;
        componentCount = 0;
        parseLength = len;
        parseCommand();
      }

      // Method to parse the command into components
      void parseCommand() {
        int startIndex = 0;
        int endIndex = command.indexOf('/');
        String newItem = "";

        while ((endIndex != -1) && (startIndex < parseLength)) {
          //To Remove
          #if DEBUG >= 2
            
            PRINTS(String(startIndex) + ":" + String(endIndex));
          #endif

          if(endIndex>startIndex){
            newItem = command.substring(startIndex, endIndex);
            newItem.trim();
            newItem.toUpperCase();
            components[componentCount++] = newItem;

          }
          startIndex = endIndex + 1;
          endIndex = command.indexOf('/', startIndex);
        }
        // Add the last component
        newItem = command.substring(startIndex);
        newItem.trim();
        newItem.toUpperCase();
        components[componentCount++] = newItem;
      }

      // Method to get the number of components
      int getComponentCount() {
        return componentCount;
      }

      // Method to get a specific component
      String getComponent(int index) {
        if (index >= 0 && index < componentCount) {
          return components[index];
        }
        return ""; // Return empty string if index is out of bounds
      }

      String* getComponents(){
        return components;
      }

      void printComponents(){
        for (uint8_t i = 0; i < getComponentCount(); i++) {
          PRINTS(String(i) + ": " + getComponent(i));
          // Serial.print(i);
          // Serial.print(": ");
          // Serial.println(getComponent(i));
        }
      }
  };


//---OSC Command Parcing

  void OSCCommandParser(String components[4]) { // Array to hold components (max 10 components).
    #if DEBUG > 3
      PRINTB();
      PRINTS("Entered OSCCommandParser"); // Print the octal value for DEBUGging purposes
    #endif
    
    //PRINT("OSC Device name",components[0]);
    PRINTB();
    if (components[1] == "BPM") {
      //Sets BPM rate and goes

      mode = 3;
      measure = 0;
      PRINTS("<----Mode 3:  Color Blink---->");
      BPMMath(components[2].toFloat());

    } else if (components[1] == "BLINK") {
      //Sets BPM rate and goes, but only a single color
      // /BEAT/BLINK/bpm/col
      // /BEAT/BLINK/120/080
      mode = 5;
      PRINTS("<----Mode 5:  Single Color Blink---->");
      BPMMath(components[2].toFloat());
      DesiredLEDr = octToByte(components[3].charAt(0));
      DesiredLEDg = octToByte(components[3].charAt(1));
      DesiredLEDb = octToByte(components[3].charAt(2));
    
      measure = 0;
      PRINTS("Mode should be" + String(mode));

    } else if (components[1] == "BPM@M") {
      //Sets BPM rate and goes, but only a single color
      // /BEAT/BPM@M/bpm/mes
      // /BEAT/BPM@M/120/18
      mode = 13;
      nextBPM = components[2].toFloat();
      nextmeasure = components[3].toInt();


    } else if (components[1] == "COLOR") {
      PRINTS("<----Mode 9:  Solid Color---->");
      
      mode = 9;
      
      DesiredLEDr = octToByte(components[2].charAt(0));
      DesiredLEDg = octToByte(components[2].charAt(1));
      DesiredLEDb = octToByte(components[2].charAt(2));
      
      measure = 0;
    } else if (components[1] == "HEART") {
      PRINTS("<----Mode 11: Heart Beat---->");
      mode = 11;
      
      DesiredLEDr = octToByte(components[2].charAt(0));
      DesiredLEDg = octToByte(components[2].charAt(1));
      DesiredLEDb = octToByte(components[2].charAt(2));
      
    } else if (components[1] == "HALT") {
      PRINTS("<----Mode 1: Halting---->");
      mode = 1;
    } else if (components[1] == "TOGGLEPP") {
      if (mode == 12){ //resume last mode
        modeResume();
      } else { // Start Pause mode
        PRINTS("<----Mode 12: Pause---->");
        modePause();
      }
    } else if (components[1] == "PAUSE") {
      PRINTS("<----Mode 12: Pause---->");
      modePause();
    } else if (components[1] == "RESUME") {
      modeResume();
    } else if (components[1] == "?") {
      varDump();
    } else if (components[1] == "Button") {
      lcdMode = (lcdMode + 1) % 4;
      lcdClear = true;
    } else {
      PRINT("[!] INVALID Parse of ", components[1]);
      PRINT("[!] With a length of", components[1].length());
    }
    currentMicroS = esp_timer_get_time();
    nextEvent = currentMicroS;
  }

//---- Beat related methods
  
  // Causes the heart beat intensitiy to be expedental
  uint8_t heartHandle(uint8_t colorVal, uint8_t brightness) {
    float colorValF = colorVal;
    float brightnessF = brightness;
    float calculatedValueF = colorValF * (brightnessF/255) * (brightnessF/255);
    int returnVal = calculatedValueF;
    return returnVal;
  }

  void heartbeat(uint8_t redval = 0, uint8_t greenval = 0, int blueval = 0) {  //Verify
  
    if (currentMicroS >= nextPulse) {
      // change the brightness for next time through the loop:
      brightness = brightness + heartFade;

      // reverse the direction of the fading at the ends of the fade:
      if (brightness <= 0 || brightness >= 250) {
        heartFade = -heartFade;
        #if DEBUG > 0
          PRINTS("Thump");
        #endif
      }
      DisplayLED.setColor(heartHandle(redval, brightness), heartHandle(greenval, brightness), heartHandle(blueval, brightness));
      nextPulse = currentMicroS + heartRate;
    }
  }

  void countbeat() {  //Verify
    //soft beats on 0,2,4,6,8,10,12.  Hard beats on 0 and 8
    if (beat % 2 == 0) {
      //digitalWrite(PIN_BEATS, HIGH);  //TODO Beat indication
      //this gives all even beats
      if (beat == 0) {  //beat 1
        #if DEBUG > 0
          PRINT("[-] Measure", measure);
        #endif
        measure++;
        if (premeasure == 1) {
          DisplayLED.G();
          premeasure = 0;
        } else {
          DisplayLED.C();
        }
      } else if (beat == 8) {  //beat 5
        #if DEBUG > 0
          PRINT("[-] Measure", measure);
        #endif
        measure++;
        DisplayLED.P();
      } else {
        DisplayLED.O();
      }
      //Lights are now on, so set current time to next off
      nextEvent = currentMicroS + metronomeON + 0.5;
    } else {  //most of the way though the beat, turn lights off
      DisplayLED.off();
      //digitalWrite(PIN_BEATS, LOW);  //TODO Beat indication
      nextEvent = currentMicroS + metronomeOFF + 0.5;
    }
    beat = beat + 1;
    if (beat > 15) beat = 0;

    #if DEBUG >= 2
      PRINT("  Beat", beat); //Add time monitor
    #endif
  }

  void countbeat(byte LEDr, byte LEDg, byte LEDb) {  //For constant color on beat //Verify
    //soft beats on 0,2,4,6,8,10,12,14,16.
    if (beat % 2 == 0) {
      //digitalWrite(PIN_BEATS, HIGH);  //TODO Beat indication
      DisplayLED.setColor(LEDr, LEDg, LEDb);

      //Lights are now on, so set current time to next off
      nextEvent = currentMicroS + metronomeON + 0.5;
      #if DEBUG >= 3
        PRINT("Solid Beat On", beat);
      #endif
    } else {  //most of the way though the beat, turn lights off
      DisplayLED.off();
      //digitalWrite(PIN_BEATS, LOW);  //TODO Beat indication
      nextEvent = currentMicroS + metronomeOFF + 0.5;
      #if DEBUG >= 3
        PRINT("Solid Beat Off", beat);
      #endif
    }
    beat = beat + 1;
    if (beat > 15) beat = 0;
  }

#if SERIALAVAL
  String SerialString = "";

  void SerialMonitor( void * pvParameters ){
    Serial.begin(115200);
    for(;;){
      while(Serial.available()) {
        currentMicroS = esp_timer_get_time();
        SerialString = Serial.readString();// read the incoming data as string
        PRINTS("[*] Processing Serial...");
        OSCParser parser(SerialString, SerialString.length());  // TRIGGER OSC RECEPTION
        #if DEBUG > 1
          parser.printComponents();
        #endif
        if (parser.getComponent(0) == "BEAT"){
          #if DEBUG > 1
            PRINT("[+]  We are", parser.getComponent(0));
          #endif 
          OSCCommandParser(parser.getComponents());
        } else {
          PRINTS("[+]  Not for me.");
        }
        #if DEBUG > 5
          PRINT("[5] Serial Monitor is running on core ", xPortGetCoreID());
          PRINT("[5] Heap Max", uxTaskGetStackHighWaterMark(nullptr));
          PRINT("[T] ∆Time", esp_timer_get_time() - currentMicroS);
        #endif
        yield();
      }
      yield();
    }
  }
#endif

void LCDUpdate(void* pvParameters) {
    while (1) {
        // Code to run in this task
        switch (lcdMode){
          case 0: //network infomation 1
          {
            if (lcdClear){
              lcd.clear();
              lcdPrint1(networkS1);
              lcdPrint2(networkS2);
              lcdClear = false;
            }
            break;
          }
          case 1: //network infomation 2
          {
            if (lcdClear){
              lcd.clear();
              lcdPrint1(networkS3);
              lcdPrint2(networkS4);
              lcdClear = false;
            }
            break;
          }
          case 2: // Measure
          {
            if (lcdClear){
              lcd.clear();
              lcdClear = false;
            }
            lcdPrint1("Measure: " + String(measure));
            break;
          }
          case 3: // Last recived command
          {
            if (lcdClear){
              lcd.clear();
              lcdClear = false;
            }
            lcdPrint1("Mode: " + String(mode));
            lcdPrint2(modeName());
            break;
          }
          default:
          {
            break;
          }
        }


      yield();
    }
}

void WiFiStationConnected(WiFiEvent_t event, WiFiEventInfo_t info){
  PRINTS("[+] Connected to AP successfully!");
}

void WiFiGotIP(WiFiEvent_t event, WiFiEventInfo_t info){
  PRINTS("[+] WiFi connected");
  PRINT("[+] IP address: ", String(WiFi.localIP()));
}

//system set up code
void setup() {
  DisplayLED.R();
   //initilize the LCD.
  lcd.init();
  // turn on LCD backlight  
  lcd.backlight();                     
  lcdPrint1("Booting");

  Serial.begin(115200);
  
  #if SERIALAVAL
    // >> Version update and print
    unsigned int ver = 0;
    byte chk = 0;
    for(int i=0; i<strlen(compile_date); ++i){
      chk += compile_date[i];
    }
    EEPROM.begin(8);
    if (EEPROM.read(0) != chk) {
      EEPROM.get(1, ver);
      EEPROM.put(1, ++ver % 255);
      EEPROM.commit(); // Commit changes to EEPROM
    }
    PRINTS("[!] Chk: " + String(chk) + ". Ver: " + String(ver) + "  built at " + String(compile_date));
    // Serial.print("[!] Chk: ");
    // Serial.print(chk);
    // Serial.print(". Ver: ");
    // Serial.print(ver);
    // Serial.print("  built at ");
    // Serial.println(compile_date);
    // Serial.println();
    // << End Version Update and Print
  #endif

  pinMode(lcdButtonPin, INPUT_PULLUP);
	attachInterrupt(lcdButtonPin, Button_isr, FALLING); 

  delay(1000);

  #if WIFI
    wifiMulti.addAP("BeatWifi", "Beat");
    //Other wifi APs hidden for security
    PRINTS("[*] Connecting to Wifi...");
    lcdPrint2("Connecting to Wifi...");
    
    if(wifiMulti.run() == WL_CONNECTED){ //WIFI is connected

    
      DisplayLED.B();
      #if SERIALAVAL
        PRINTS("[*] Connected to the WiFi network");
        get_network_info();
      #endif

      if (udp.listen(udpPort)) {
        PRINTS("[*] WiFi connected. UDP Listening on IP:" + WiFi.localIP().toString() + ":" + String(udpPort) + " on " + String(WiFi.SSID()));
        networkS1 = "Connected.  IP:";
        networkS2 = WiFi.localIP().toString();
        networkS3 = "Port: " + String(udpPort) + " on:";
        networkS4 = String(WiFi.SSID());

        udp.onPacket([](AsyncUDPPacket packet) {
          #if SERIALAVAL
            PRINTS("[+]  UDP Packet Type: " + packet.isBroadcast() ? "Broadcast" : packet.isMulticast() ? "Multicast": "Unicast");
            PRINTS("[+]  From: " + packet.remoteIP().toString() + ":" + String(packet.remotePort()));
            PRINTS("[+]  To: " + packet.localIP().toString() + ":" + String(packet.localPort()));
            PRINTS("[+]  Length: " + String(packet.length())); 
          #endif

          String udpString = (const char*)packet.data();
          PRINT("[+]  Data: ", udpString);

          OSCParser parser(udpString, packet.length());
          parser.printComponents();
          OSCCommandParser(parser.getComponents());
          

          packet.printf("[+]  Got %u bytes of data", packet.length());
        });
      }
      WiFi.onEvent(WiFiStationConnected, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_CONNECTED);
      WiFi.onEvent(WiFiGotIP, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_GOT_IP);
      //WiFi.onEvent(WiFiStationDisconnected, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_DISCONNECTED);

      DisplayLED.G();  //Good to go
      
      
    } else {
      DisplayLED.Y();  //Well no wifi today
      PRINTS("[*] Could not connect to WiFi");
    }
  #else
    PRINTS("[*] Wifi Disabled");
    DisplayLED.O();
  #endif

  #if SERIALAVAL
  xTaskCreatePinnedToCore(
                SerialMonitor,   /* Task function. */
                "SerialMonitor",     /* name of task. */
                2000,       /* Stack size of task */
                NULL,        /* parameter of the task */
                1,           /* priority of the task */
                &TaskSerial,      /* Task handle to keep track of created task */
                1);          /* pin task to core 0 */
  #endif

  xTaskCreatePinnedToCore(
                LCDUpdate,   /* Task function. */
                "LCD Updater",     /* name of task. */
                4000,       /* Stack size of task */
                NULL,        /* parameter of the task */
                0,           /* priority of the task */
                &TaskLCD,      /* Task handle to keep track of created task */
                0);          /* pin task to core 0 */
  
}
 

void loop() {

  currentMicroS = esp_timer_get_time();
 //Its event time!
  if (((currentMicroS >= nextEvent) || (lastmode != mode)) && ((nextEvent > 0))) {
    #if DEBUG >= 3
      printtimes();
    #endif


    if (lastmode != mode) {
      PRINT("[*] New Mode", mode);
      PRINT("[+]  Mode Name", modeName());
      lastmode = mode;
    }

    // Modes:  Even = Active mode, Even+1 = Newly set mode
      // 0:  Doing Nothing
      // 2:  normal blink
      // 4:  single color blink
      // 6:  preset beat blink
      // 8:  solid color
      // 10: heartbeat

    switch (mode) { //We are within the nextEvents compare
      case 0:
        break;
      case 1: //Time to stop and reset
        DisplayLED.off();
        mode = 0;
        break;
      case 2: //We are running beat mode, we are at event time, do beat
        countbeat();
        break;
      case 3: //Start the normal blink mode
        beat = 0;
        premeasure = 0;
        countbeat();
        mode = 2;
        break;
      case 4: //We are running beat mode using a set color, we are at event time, do beat
        countbeat(DesiredLEDr,DesiredLEDg,DesiredLEDb);
        break;
      case 5: //Start the blink mode using a single color
        beat = 0;
        premeasure = 0;
        countbeat(DesiredLEDr,DesiredLEDg,DesiredLEDb);
        mode = 4;
        break;
      case 6: //
        countbeat();
        break;
      case 7:  //leftovers from starting beat before a measure
        beat = 0;
        premeasure = 0;
        countbeat();
        mode = 2;
        break;
      case 8:  //Set solid color
      case 9:
        DisplayLED.setColor(DesiredLEDr,DesiredLEDg,DesiredLEDb);
        beat = 0;
        mode = 8;
        break;
      case 10: //Heartbeat
        heartbeat(DesiredLEDr,DesiredLEDg,DesiredLEDb);
        break;
      case 11: //Heeartbeat.  This brings in the new color to hold, reset beat to 0 for next time, make case 1
        heartbeat(DesiredLEDr,DesiredLEDg,DesiredLEDb);
        beat = 0;
        mode = 10;
        break;
      case 12: //We are in a hold mode
        if (currentMicroS % 50000 == 0) PRINT("Holding in Mode 12", currentMicroS);
        break;
      case 13: //At an upcomming measure, change rate
        if (measure == nextmeasure){
          PRINTB();
          PRINTS("<----Mode 13:  Going to new Measure---->");
          BPMMath(nextBPM);
          
          measure == nextmeasure;
          mode = 2;
        }
        countbeat();
        break;
      default:
        PRINT("Mode was out of bounds at",mode);
        mode = 1;
        break;
    }
  }
  
}