#define CLK_PIN1 14  // ESP32 pin GPIO14 connected to the first rotary encoder's CLK pin
#define DT_PIN1  13  // ESP32 pin GPIO13 connected to the first rotary encoder's DT pin
#define SW_PIN1  15  // ESP32 pin GPIO15 connected to the first rotary encoder's SW pin

// Pins for Encoder 2
#define CLK_PIN2 17  // ESP32 pin GPIO18 connected to the second rotary encoder's CLK pin
#define DT_PIN2  16  // ESP32 pin GPIO19 connected to the second rotary encoder's DT pin
#define SW_PIN2  18  // ESP32 pin GPIO21 connected to the second rotary encoder's SW pin

// Define LED pins
#define LED1_PIN 19  // LED1 connected to GPIO19
#define LED2_PIN 21  // LED2 connected to GPIO21

#define DIRECTION_CW  0   // clockwise direction
#define DIRECTION_CCW 1   // counter-clockwise direction

// Encoder 1 variables
int counter1 = 1;  // Start counting from 1
int direction1 = DIRECTION_CW;
int CLK_state1;
int prev_CLK_state1;
bool isCountingEnabled1 = false;  // State to enable or disable counting for Encoder 1

// Encoder 2 variables
int counter2 = 1;  // Start counting from 1
int direction2 = DIRECTION_CW;
int CLK_state2;
int prev_CLK_state2;
bool isCountingEnabled2 = false;  // State to enable or disable counting for Encoder 2

// Button debounce variables
int buttonState1;
int lastButtonState1 = LOW;
unsigned long lastDebounceTime1 = 0;
unsigned long debounceDelay1 = 50;

int buttonState2;
int lastButtonState2 = LOW;
unsigned long lastDebounceTime2 = 0;
unsigned long debounceDelay2 = 50;

int prevCounter1 = -1;  // Initialize with an invalid value to ensure first print
int prevCounter2 = -1;

int currentMode = 0;  // Start in Mode 0
unsigned long mode2StartTime = 0;
bool mode2Active = false;

void setup() {
  Serial.begin(9600);

  // configure encoder pins as inputs for Encoder 1
  pinMode(CLK_PIN1, INPUT);
  pinMode(DT_PIN1, INPUT);
  pinMode(SW_PIN1, INPUT_PULLUP);  // configure button pin as input with pull-up resistor for Encoder 1

  // configure encoder pins as inputs for Encoder 2
  pinMode(CLK_PIN2, INPUT);
  pinMode(DT_PIN2, INPUT);
  pinMode(SW_PIN2, INPUT_PULLUP);  // configure button pin as input with pull-up resistor for Encoder 2

  // configure LED pins as outputs
  pinMode(LED1_PIN, OUTPUT);
  pinMode(LED2_PIN, OUTPUT);

  // read the initial state of the rotary encoder's CLK pin for both encoders
  prev_CLK_state1 = digitalRead(CLK_PIN1);
  prev_CLK_state2 = digitalRead(CLK_PIN2);
}

void loop() {
  // Debounce logic for SW1 and SW2
  int reading1 = digitalRead(SW_PIN1);
  int reading2 = digitalRead(SW_PIN2);

  // Update debounce timers for SW1
  if (reading1 != lastButtonState1) {
    lastDebounceTime1 = millis();
  }
  if ((millis() - lastDebounceTime1) > debounceDelay1) {
    if (reading1 != buttonState1) {
      buttonState1 = reading1;

      // Handle SW1 press in different modes
      if (buttonState1 == LOW) {
        handleSW1Press();
      }
    }
  }
  lastButtonState1 = reading1;  // Save SW1 reading

  // Update debounce timers for SW2
  if (reading2 != lastButtonState2) {
    lastDebounceTime2 = millis();
  }
  if ((millis() - lastDebounceTime2) > debounceDelay2) {
    if (reading2 != buttonState2) {
      buttonState2 = reading2;

      // Handle SW2 press in different modes
      if (buttonState2 == LOW) {
        handleSW2Press();
      }
    }
  }
  lastButtonState2 = reading2;  // Save SW2 reading

  // Check for timeout in Mode 2 (3-second flash, then return to Mode 0)
  if (mode2Active && millis() - mode2StartTime > 3000) {  // 3 seconds
    Serial.println("Mode 2 timeout, returning to Mode 0");
    currentMode = 0;
    mode2Active = false;
    updateModes();
  }

  // Continuously update LEDs based on the current mode
  updateModes();
}

// Function to handle SW1 press
void handleSW1Press() {
  switch (currentMode) {
    case 0:
      // Enter Mode 1, enable Encoder 1, ignore SW2
      Serial.println("Entering Mode 1, enabling Encoder 1");
      currentMode = 1;
      isCountingEnabled1 = true;  // Enable counting for Encoder 1
      isCountingEnabled2 = false; // Disable Encoder 2
      updateModes();
      break;

    case 1:
      // Enter Mode 2, set 3-second flash timer for LED1
      Serial.println("Entering Mode 2");
      currentMode = 2;
      mode2StartTime = millis();  // Start 3-second timer
      mode2Active = true;
      isCountingEnabled1 = true;  // Keep Encoder 1 enabled in Mode 2
      updateModes();
      break;

    case 2:
      // Manually exit Mode 2 and return to Mode 0
      Serial.println("Exiting Mode 2 manually, returning to Mode 0");
      currentMode = 0;
      mode2Active = false;
      updateModes();
      break;

    default:
      break;
  }
}

// Function to handle SW2 press
void handleSW2Press() {
  switch (currentMode) {
    case 0:
      // Enter Mode 3, enable Encoder 2, ignore SW1
      Serial.println("Entering Mode 3, enabling Encoder 2");
      currentMode = 3;
      isCountingEnabled2 = true;  // Enable counting for Encoder 2
      isCountingEnabled1 = false; // Disable Encoder 1
      updateModes();
      break;

    case 3:
      // Enter Mode 4, still listen to SW2
      Serial.println("Entering Mode 4");
      currentMode = 4;
      isCountingEnabled2 = true;  // Keep Encoder 2 enabled in Mode 4
      updateModes();
      break;

    case 4:
      // Return to Mode 0, disable encoders
      Serial.println("Returning to Mode 0");
      currentMode = 0;
      isCountingEnabled1 = false; // Disable both encoders
      isCountingEnabled2 = false;
      updateModes();
      break;

    default:
      break;
  }
}

// Function to update LED states based on the current mode
void updateModes() {
  switch (currentMode) {
    case 0:
      // Mode 0: Both LEDs off
      digitalWrite(LED1_PIN, LOW);
      digitalWrite(LED2_PIN, LOW);
      break;

    case 1:
      // Mode 1: LED1 on, LED2 off
      digitalWrite(LED1_PIN, HIGH);
      digitalWrite(LED2_PIN, LOW);
      if (isCountingEnabled1) 
      {
          int currentCounterValue1 = rotaryCounter1();  // Get the counter value for Encoder 1

          // Only print if the counter value has changed
          if (currentCounterValue1 != prevCounter1) {
            Serial.print("Encoder 1 current counter value: ");
            Serial.println(currentCounterValue1);
            prevCounter1 = currentCounterValue1;  // Update previous counter value
          }
      }
      break;

    case 2:
      // Mode 2: LED1 flashing
      if ((millis() - mode2StartTime) % 500 < 250) {
        digitalWrite(LED1_PIN, HIGH);  // Flash LED1
      } else {
        digitalWrite(LED1_PIN, LOW);
      }
      digitalWrite(LED2_PIN, LOW);  // Ensure LED2 is off
      break;

    case 3:
      // Mode 3: LED1 off, LED2 on
      digitalWrite(LED1_PIN, LOW);
      digitalWrite(LED2_PIN, HIGH);

      if (isCountingEnabled2)
      {
          int currentCounterValue2 = rotaryCounter2();  // Get the counter value for Encoder 2

          // Only print if the counter value has changed
          if (currentCounterValue2 != prevCounter2) {
            Serial.print("Encoder 2 current counter value: ");
            Serial.println(currentCounterValue2);
            prevCounter2 = currentCounterValue2;  // Update previous counter value
          }
      }
      break;

    case 4:
      // Mode 4: LED2 flashing
      digitalWrite(LED1_PIN, LOW);  // Ensure LED1 is off
      if (millis() % 500 < 250) {
        digitalWrite(LED2_PIN, HIGH);  // Flash LED2
      } else {
        digitalWrite(LED2_PIN, LOW);
      }
      break;

    default:
      // Safety: Turn off both LEDs in any undefined mode
      digitalWrite(LED1_PIN, LOW);
      digitalWrite(LED2_PIN, LOW);
      break;
  }
}


// Function for handling rotary encoder counting for Encoder 1
int rotaryCounter1() {
  // read the current state of the rotary encoder's CLK pin
  CLK_state1 = digitalRead(CLK_PIN1);

  // If the state of CLK has changed, a pulse occurred
  if (CLK_state1 != prev_CLK_state1 && CLK_state1 == HIGH) {
    // If the DT state is HIGH, the encoder is rotating counter-clockwise => decrease the counter
    if (digitalRead(DT_PIN1) == HIGH) {
      if (counter1 > 1) {
        counter1--;  // Only decrease if counter is greater than 1
        direction1 = DIRECTION_CCW;
      }
    } else {
      if (counter1 < 8) {
        counter1++;  // Only increase if counter is less than 8
        direction1 = DIRECTION_CW;
      }
    }
    
    if (direction1 == DIRECTION_CW)
      Serial.print("Clockwise");
    else
      Serial.print("Counter-clockwise");
  }

  // Save last CLK state
  prev_CLK_state1 = CLK_state1;

  return counter1;
}

// Function for handling rotary encoder counting for Encoder 2
int rotaryCounter2() {
  // read the current state of the rotary encoder's CLK pin
  CLK_state2 = digitalRead(CLK_PIN2);

  // If the state of CLK has changed, a pulse occurred
  if (CLK_state2 != prev_CLK_state2 && CLK_state2 == HIGH) {
    // If the DT state is HIGH, the encoder is rotating counter-clockwise => decrease the counter
    if (digitalRead(DT_PIN2) == HIGH) {
      if (counter2 > 1) {
        counter2--;  // Only decrease if counter is greater than 1
        direction2 = DIRECTION_CCW;
      }
    } else {
      if (counter2 < 8) {
        counter2++;  // Only increase if counter is less than 8
        direction2 = DIRECTION_CW;
      }
    }
    if (direction2 == DIRECTION_CW)
      Serial.print("Clockwise");
    else
      Serial.print("Counter-clockwise");
  }
  // Save last CLK state
  prev_CLK_state2 = CLK_state2;

  return counter2;
}