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


// OLED display dimensions
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// I2C address for the OLED display
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Rotary Encoder pins
#define SW_PIN 17
#define CLK_PIN 19
#define DT_PIN 18

// Variables for rotary encoder
int lastClk = HIGH;
bool encoderPressed = false;
int menuIndex = 0;  // For menu navigation
int currentScreen = 1;  // 1 = Main Menu, 2 = Sub Menu

// New variable for submenu navigation
int subMenuIndex = 0; // For submenu navigation
int triMenuIndex = 0;
int pretriMenuIndex=0;
int subPress=3;
int rot=0;
int timeflag=0;

unsigned long currentMillis = 0;
unsigned long lastUpdate = 0;
int rotationState = 2; // 0 = Left, 1 = Right, 2 = No rotation

static const unsigned char PROGMEM image_ButtonRightSmall_bits[] = {0x80,0xc0,0xe0,0xc0,0x80};
static const unsigned char PROGMEM image_ButtonCenter__copy__bits[] = {0x38,0x44,0xba,0xba,0xba,0x44,0x38};
static const unsigned char PROGMEM image_ButtonLeftSmall_bits[] = {0x20,0x60,0xe0,0x60,0x20};

//static const unsigned char PROGMEM image_download_bits[] = {0x0f,0xff,0xc0,0x1f,0xff,0x80,0x3f,0xff,0x00,0x3f,0xfe,0x00,0x3f,0xfe,0x40,0x3f,0xfc,0x40,0x3f,0xf8,0xc0,0x3f,0xf1,0xc0,0x3f,0xe3,0xc0,0x3f,0xe3,0xc0,0x3f,0xf3,0xc0,0x3f,0xf1,0xc0,0x3f,0xf8,0xc0,0x3f,0xfc,0x40,0x3f,0xfe,0x00,0x3f,0xff,0x00,0x1f,0xff,0x00,0x1f,0xff,0x80,0x07,0xff,0xc0,0x00,0x00,0x00};

void setup() {
  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  
  // Initialize encoder pins
  pinMode(SW_PIN, INPUT_PULLUP);
  pinMode(CLK_PIN, INPUT);
  pinMode(DT_PIN, INPUT);
  Serial.begin(9600);
  currentMillis = millis();
  
  // Show initial menu
  displayMainMenu();
}

void loop() {
  readEncoder();  // Monitor encoder input
  currentMillis = millis();



  // If the button is pressed, navigate to the submenu based on selected item
  if (encoderPressed) {
    if (currentScreen == 1) {
      // Navigate to submenu based on menuIndex
      if (menuIndex == 0) { // If P1 is selected
        subMenuIndex = 0;
        pretriMenuIndex=0;
        triMenuIndex=0;
        displayP1SubMenu();
        currentScreen = 2; // Set the screen to submenu
         // Reset submenu index
         subPress=3;
      } else if (menuIndex == 1) { // If P2 is selected
        subMenuIndex = 0; 
        triMenuIndex=0;
        pretriMenuIndex=0;
        displayP2SubMenu();
        currentScreen = 3; // Set the screen to submenu
        // Reset submenu index
        subPress=3;
      }
    } else if (currentScreen==2||currentScreen==3){
      // Check if "Back" is selected in submenu
      if (subMenuIndex == 3) {
        displayMainMenu();
        currentScreen = 1; // Set the screen back to main menu
        //menuIndex = 0;   // Reset main menu index
      }
      else if (subMenuIndex < 3) {
        //subMenuIndex = 0;
        triMenuIndex=0;
       pretriMenuIndex=0;
        displayTriMenu(rot);
        currentScreen=4;
      }
    }
    else if(currentScreen==4)
    {
       //subMenuIndex = 0;
       triMenuIndex=0;
       pretriMenuIndex=0;
       rot=0;
       if(menuIndex==0)displayP1SubMenu();
       else displayP2SubMenu();
      currentScreen = 2; // Set the screen to submenu
       
       

    }
    encoderPressed = false;
  }



 
  //Serial.println(currentScreen);
}

void  updateRotationStatus() {
    Serial.println("d");

 if(timeflag==1){
  Serial.println("k");
   currentMillis = millis();
  if (currentMillis - lastUpdate > 300) {  // Only update every 200ms
   
   rot=0;
   displayTriMenu(rot);


    lastUpdate = currentMillis;
    timeflag=2;
  }
  }
  }


// Function to handle rotary encoder inputs
void readEncoder() {
  int clkValue = digitalRead(CLK_PIN);
  //Serial.println(rot);
  
  if (clkValue != lastClk && clkValue == LOW) {  // Detect a rotation
    if (digitalRead(DT_PIN) != clkValue) {
      if (currentScreen == 1) {
        menuIndex++;
      } else if(currentScreen==2||currentScreen==3){
        subMenuIndex++;
        }
        else{
        triMenuIndex++;
      }
    } else {
      if (currentScreen == 1) {
        menuIndex--;
      } else if(currentScreen==2||currentScreen==3) {
        subMenuIndex--;}
        else{
        triMenuIndex--;
      }
    }
    
    // Limit menuIndex to valid range for the main menu
    if (currentScreen == 1) {
      if (menuIndex < 0) menuIndex = 1;  // Only 2 options in Main Menu (P1, P2)
      if (menuIndex > 1) menuIndex = 0;   // Wrap around
    } 
    
    // Limit subMenuIndex to valid range for the submenus
    else if (currentScreen == 2) {
      if (subMenuIndex < 0) subMenuIndex = 3;  // 3 options + 1 Back option
      if (subMenuIndex > 3) subMenuIndex = 0;   // Wrap around
    }
    else if (currentScreen == 3) {
      if (subMenuIndex < 0) subMenuIndex = 3;  // 3 options + 1 Back option
      if (subMenuIndex > 3) subMenuIndex = 0;   // Wrap around
    }
    else if (currentScreen == 4) {

      if(triMenuIndex>pretriMenuIndex)rot=1;
      else if(triMenuIndex<pretriMenuIndex)rot=2;
      else rot=0;
      pretriMenuIndex=triMenuIndex;
      //rot=triMenuIndex;
      timeflag=1;
      lastUpdate = currentMillis;
      delay(10);
    
    }

    // Update the display according to the current screen
    if (currentScreen == 1) {
      displayMainMenu();
    } else if(currentScreen==2||currentScreen==3) {
      // For P1 or P2 submenu, call the specific submenu functions
      if (menuIndex == 0) {
        displayP1SubMenu();
      } else if (menuIndex == 1) {
        displayP2SubMenu();
      }
    }
    else if(currentScreen == 4)
    {// pretriMenuIndex++;
      displayTriMenu(rot);
    }
    //Serial.println(rot);
  }
   if (timeflag==1)
  {
    updateRotationStatus();
  }

  lastClk = clkValue;

  // Detect encoder button press
  if (digitalRead(SW_PIN) == LOW) {
    delay(200);  // Debounce
    encoderPressed = true;
  }
}

// Function to display the title at the top of the screen
void displayHeader() {
  display.setTextSize(1);  // Set font size to 1 for visibility
  display.setTextColor(WHITE);
  display.setCursor(41, 3); // Position at the top
  display.println("EMBEDITE");
  //display.drawBitmap(54, 42, image_download_bits, 20, 20, 1);
}

// Function to display Main Menu (Screen 1)
void displayMainMenu() {
  display.clearDisplay();
  displayHeader();
  display.setTextSize(2);

  // Menu item 1 (P1) - Left side
  if (menuIndex == 0) {
    // Draw a larger white rectangle behind the text
    display.fillRect(12, 27, 40, 30, WHITE);
    display.setTextColor(BLACK);  // Text in black over white background
  } else {
    display.setTextColor(WHITE);  // Regular text
  }

  display.setCursor(21, 34);  // Position P1 on the left
  display.println("P1");

  // Menu item 2 (P2) - Right side
  if (menuIndex == 1) {
    // Draw a larger white rectangle behind the text
    display.fillRect(76, 27, 40, 30, WHITE);
    display.setTextColor(BLACK);  // Text in black over white background
  } else {
    display.setTextColor(WHITE);  // Regular text
  }

  display.setCursor(86, 34);  // Position P2 on the right
  display.println("P2");

  display.display();
}

// Function to display P1 Sub Menu
void displayP1SubMenu() {
  display.clearDisplay();
  displayHeader(); // Display "EMBEDITE" at the top
  display.setTextSize(1);
  display.setTextColor(WHITE);

  // Sub-menu items for P1
  //display.setCursor(0, 25);
  if (subMenuIndex == 0) {
    display.fillRect(2, 12, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 14);
    display.print("P1-FOC");
    
    //if(subPress==0){
    //display.drawRect(50, 12, 76, 10, 1);}
  } else {
    display.fillRect(2, 12, 42, 10,0);
    display.setTextColor(1);
    display.setCursor(6, 14);
    display.print("P1-FOC");
    display.drawRect(50, 12, 76, 10, 0);
  }



  if (subMenuIndex == 1) {
    display.fillRect(2, 24, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 26);
    display.print("P1-VLS");
  } else {
    display.fillRect(2, 24, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(6, 26);
    display.print("P1-VLS");
  }


  if (subMenuIndex == 2) {
    display.fillRect(2, 36, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 38);
    display.print("P1-HLS");
  } else {
    display.fillRect(2, 36, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(6, 38);
    display.print("P1-HLS");
  }
  

  //display.setCursor(10, 53);
  if (subMenuIndex == 3) {
    display.fillRect(2, 48, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(10, 50);
    display.print("BACK");
  } else {
    display.fillRect(2, 48, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(10, 50);
    display.print("BACK");
  }
  //display.println("Back");

  display.display();
}

// Function to display P2 Sub Menu
void displayP2SubMenu() {
  display.clearDisplay();
  displayHeader(); // Display "EMBEDITE" at the top
  display.setTextSize(1);
  display.setTextColor(WHITE);

  // Sub-menu items for P1
  //display.setCursor(0, 25);
  if (subMenuIndex == 0) {
    display.fillRect(2, 12, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 14);
    display.print("P2-FOC");
  } else {
    display.fillRect(2, 12, 42, 10,0);
    display.setTextColor(1);
    display.setCursor(6, 14);
    display.print("P2-FOC");
  }



  if (subMenuIndex == 1) {
    display.fillRect(2, 24, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 26);
    display.print("P2-VLS");
  } else {
    display.fillRect(2, 24, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(6, 26);
    display.print("P2-VLS");
  }


  if (subMenuIndex == 2) {
    display.fillRect(2, 36, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 38);
    display.print("P2-HLS");
  } else {
    display.fillRect(2, 36, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(6, 38);
    display.print("P2-HLS");
  }
  

  //display.setCursor(10, 53);
  if (subMenuIndex == 3) {
    display.fillRect(2, 48, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(10, 50);
    display.print("BACK");
  } else {
    display.fillRect(2, 48, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(10, 50);
    display.print("BACK");
  }
  //display.println("Back");

  display.display();
} //: modify this code such a way that if sw is pressed in any one of the submenu index enter to page 3 and exit back if again sw is pressed
void displayTriMenu(int rot1)
{ //Serial.println(++rot);
  display.clearDisplay();
  displayHeader(); // Display "EMBEDITE" at the top
  display.setTextSize(1);
  display.setTextColor(WHITE);

  // Sub-menu items for P1
  //display.setCursor(0, 25);
  if (subMenuIndex == 0) {
    display.fillRect(2, 12, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 14);

    if(menuIndex==0)display.print("P1-FOC");
    else display.print("P2-FOC");

    display.drawRect(50, 12, 76, 10, 1);
    display.setTextColor(1);
     display.setCursor(84, 14);
     //int a=updateRotationStatus(rot);
    display.print(rot);
    // delay(300);
    // a=updateRotationStatus(rot);
    // display.print(a);


   }
   else {
    display.fillRect(2, 12, 42, 10,0);
    display.setTextColor(1);
    display.setCursor(6, 14);
    if(menuIndex==0)display.print("P1-FOC");
    else display.print("P2-FOC");
    display.drawRect(50, 12, 76, 10, 0);
  }



  if (subMenuIndex == 1) {
    display.fillRect(2, 24, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 26);

    if(menuIndex==0)display.print("P1-VLS");
    else display.print("P2-VLS");

    display.drawRect(50, 24, 76, 10, 1);
    display.setTextColor(1);
     display.setCursor(84, 26);
    display.print(rot1);
    
  } else {
    display.fillRect(2, 24, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(6, 26);
    if(menuIndex==0)display.print("P1-VLS");
    else display.print("P2-VLS");
  }


  if (subMenuIndex == 2) {
    display.fillRect(2, 36, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(6, 38);

    if(menuIndex==0)display.print("P1-HLS");
    else display.print("P2-HLS");

    display.drawRect(50, 36, 76, 10, 1);
    display.setTextColor(1);
     display.setCursor(84, 38);
    display.print(rot1);
  } else {
    display.fillRect(2, 36, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(6, 38);
    if(menuIndex==0)display.print("P1-HLS");
    else display.print("P2-HLS");
  }
  

  //display.setCursor(10, 53);
  if (subMenuIndex == 3) {
    display.fillRect(2, 48, 42, 10, 1);
    display.setTextColor(0);
    display.setCursor(10, 50);
    display.print("BACK");
  } else {
    display.fillRect(2, 48, 42, 10, 0);
    display.setTextColor(1);
    display.setCursor(10, 50);
    display.print("BACK");
  }
  //display.println("Back");

  display.display();
}