#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// OLED display dimensions (128x64)
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Declare SSD1306 display object (I2C address 0x3C for OLED)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Encoder pins
const int RotaryCLK = 15;  // CLK pin on the rotary encoder
const int RotaryDT = 16;   // DT pin on the rotary encoder
const int PushButton = 17; // Button to enter/exit menu

int menuCounter = 0; // Counts clicks of the rotary encoder between menu items (0-3)
int menu1_Value = 0;
int menu2_Value = 0;
int menu3_Value = 0;
int menu4_Value = 0;

bool menu1_selected = false;
bool menu2_selected = false;
bool menu3_selected = false;
bool menu4_selected = false;

int CLKNow, CLKPrevious;
int DTNow, DTPrevious;

unsigned long lastButtonPressTime = 0;  // For debouncing
int buttonCounter = 0;
bool buttonPressed = false; // To track button state
unsigned long debounceDelay = 100;  // 300ms debounce time
int lastButtonState = 0;
bool currentButtonState; 

int currentPage = 0;


bool refreshDisplay = true; // To refresh OLED display


void setup() {
  // Initialize encoder pins
  pinMode(RotaryCLK, INPUT_PULLUP);
  pinMode(RotaryDT, INPUT_PULLUP);
  pinMode(PushButton, INPUT_PULLUP);

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("SSD1306 allocation failed"));
        for (;;);  // Loop forever if OLED fails
    }
    display.clearDisplay();
    display.display();

  // Welcome screen
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Menu Demo");
  display.setCursor(0, 10);
  display.println("Rotary Encoder");
  display.setCursor(0, 20);
  display.println("OLED Version");
  display.display();
  delay(3000); // Display message for 3 seconds
  display.clearDisplay();

  CLKPrevious = digitalRead(RotaryCLK);
  DTPrevious = digitalRead(RotaryDT);

  attachInterrupt(digitalPinToInterrupt(RotaryCLK), rotate, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PushButton), pushButton, FALLING);

  //printOLED(); // Initialize the static parts of the display
}

void loop() {     
      
      countPresses();
 

  if (refreshDisplay) {    
    updateOLED(); // Refresh values on the display

    if (!(menu1_selected || menu2_selected || menu3_selected || menu4_selected)) {
      updateCursorPosition(); // Update the cursor position
    } 
    refreshDisplay = false;
    }
  

  delay(50);
}

void rotate() {
  CLKNow = digitalRead(RotaryCLK);
  
  if (CLKNow != CLKPrevious && CLKNow == LOW) {
    if (digitalRead(RotaryDT) != CLKNow) {
      if (currentPage == 0) currentPage = 1;
      if (menu1_selected && menu1_Value < 100) menu1_Value++;
      else if (menu2_selected && menu2_Value < 100) menu2_Value++;
      else if (menu3_selected && menu3_Value < 100) menu3_Value++;
      else if (menu4_selected && menu4_Value < 100) menu4_Value++;
      else if (!menu1_selected && !menu2_selected && !menu3_selected && !menu4_selected && buttonCounter >0) menuCounter++;
      if (menuCounter > 3) menuCounter = 0;
    } else {
      if(currentPage == 1 && buttonCounter == 0) currentPage = 0;
      if (menu1_selected && menu1_Value > 0) menu1_Value--;
      else if (menu2_selected && menu2_Value > 0) menu2_Value--;
      else if (menu3_selected && menu3_Value > 0) menu3_Value--;
      else if (menu4_selected && menu4_Value > 0) menu4_Value--;
      else if (!menu1_selected && !menu2_selected && !menu3_selected && !menu4_selected && buttonCounter) menuCounter--;
      if (menuCounter < 0) menuCounter =3;
    }
  }

  CLKPrevious = CLKNow;
  refreshDisplay = true;
}

void countPresses(){
  
  currentButtonState = digitalRead(PushButton);

   // Check if the button state has changed from the last loop iteration
  if (currentButtonState != lastButtonState) {
    // Check if the button was pressed (state goes from HIGH to LOW)
    if (currentButtonState == LOW && currentPage ==1) {
      buttonCounter++;  // Increment the press count
      
    }
    lastButtonState = currentButtonState;
  }
  
  
  if (buttonCounter > 2){
     buttonCounter = 0;
     menuCounter = 0;
  }

}

void pushButton() {

if(buttonCounter >0){
  switch (menuCounter) {
    case 0: menu1_selected = !menu1_selected; break;
    case 1: menu2_selected = !menu2_selected; break;
    case 2: menu3_selected = !menu3_selected; break;
    case 3: menu4_selected = !menu4_selected; break;
  }
}
  refreshDisplay = true;
 
}

//void printOLED() {
  // Print static menu names
 // if (currentPage == 1){
 // display.clearDisplay();
 // display.setTextSize(1);
 // display.setTextColor(SSD1306_WHITE);
 // display.setCursor(10, 0); display.println("Setpoint");
 // display.setCursor(10, 16); display.println("Kp.Value");
 // display.setCursor(10, 32); display.println("Ki.Value");
//  display.setCursor(10, 48); display.println("Kd.Value");
 // display.display();
//  }
//}

void updateOLED() {
 display.clearDisplay();
  if (currentPage == 1){
  // Menu 1
  if (menu1_selected) {
    display.fillRect(60, 0, 18, 7, SSD1306_WHITE); // Inverted background
    display.setCursor(10, 0); display.println("Setpoint");
    display.setTextColor(SSD1306_BLACK); // Text in black
    display.setCursor(60, 0); display.print(menu1_Value);
    display.setTextColor(SSD1306_WHITE); // Reset text color to white
  } else {
    display.setCursor(10, 0); display.println("Setpoint");
    display.setCursor(60, 0); display.print(menu1_Value);
  }
  
  // Menu 2
  if (menu2_selected) {
    display.fillRect(60, 16, 18, 7, SSD1306_WHITE); // Inverted background
    display.setCursor(10, 16); display.println("Kp.Value");
    display.setTextColor(SSD1306_BLACK); // Text in black
    display.setCursor(60, 16); display.print(menu2_Value);
    display.setTextColor(SSD1306_WHITE); // Reset text color to white
  } else {
    display.setCursor(10, 16); display.println("Kp.Value");
    display.setCursor(60, 16); display.print(menu2_Value);
  }

  // Menu 3
  if (menu3_selected) {
    display.fillRect(60, 32, 18, 7, SSD1306_WHITE); // Inverted background
    display.setCursor(10, 32); display.println("Ki.Value");
    display.setTextColor(SSD1306_BLACK); // Text in black
    display.setCursor(60, 32); display.print(menu3_Value);
    display.setTextColor(SSD1306_WHITE); // Reset text color to white
  } else {
    display.setCursor(10, 32); display.println("Ki.Value");
    display.setCursor(60, 32); display.print(menu3_Value);
  }

  // Menu 4
  if (menu4_selected) {
    display.fillRect(60, 48, 18, 7, SSD1306_WHITE); // Inverted background
    display.setCursor(10, 48); display.println("Kd.Value");
    display.setTextColor(SSD1306_BLACK); // Text in black
    display.setCursor(60, 48); display.print(menu4_Value);
    display.setTextColor(SSD1306_WHITE); // Reset text color to white
  } else {
    display.setCursor(10, 48); display.println("Kd.Value");
    display.setCursor(60, 48); display.print(menu4_Value);
  }

  display.setCursor(90, 0); display.print("Cnt: "); display.print(menuCounter);
  display.setCursor(90, 16); display.print("Bct: "); display.print(buttonCounter);
  display.setCursor(90, 32); display.print("Pag: "); display.print(currentPage);
  }
  
  if (currentPage == 0){
  display.setCursor(90, 32); display.print("Pag: "); display.print(currentPage);
  }

  display.display();

}

void updateCursorPosition() {
  if (buttonCounter >0 && currentPage == 1){
    display.setCursor(0, 0); display.print(menuCounter == 0 ? ">" : " ");
    menuCounter == 0 ? display.drawRoundRect(0, 0, 58, 11, 1, WHITE) : display.drawRoundRect(0, 0, 58, 11, 1, BLACK);
    display.setCursor(0, 16); display.print(menuCounter == 1 ? ">" : " ");
    display.setCursor(0, 32); display.print(menuCounter == 2 ? ">" : " ");
    display.setCursor(0, 48); display.print(menuCounter == 3 ? ">" : " ");
    display.display();
}
}