#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

#define BTN_UP 2
#define BTN_SELECT 3
#define BTN_DOWN 4
#define BTN_BACK 5

const int buttons[] = {BTN_UP, BTN_SELECT, BTN_DOWN, BTN_BACK};
const char* buttonNames[] = {"Up", "Select", "Down", "Back"};


int menuIndex = 0;
int totalMenu = 3;
String menuItems[] = {"Sensor", "Parameters", "Start"};


float kp = 0;
float ki = 0;
float kd = 0;
int speedMotor = 0;

bool editingParams = false;

// Button state tracking
bool currentButtonState[4] = {HIGH, HIGH, HIGH, HIGH};
bool lastStableState[4] = {HIGH, HIGH, HIGH, HIGH};
unsigned long lastDebounceTime[4] = {0, 0, 0, 0};
const unsigned long debounceDelay = 30;
const int BUTTON_DEBOUNCE_MS = 150;

inline bool isButtonPressed(uint8_t pin) {
  static uint8_t lastPin = 0;
  static unsigned long lastPressTime = 0;
  
  if (digitalRead(pin) == LOW) {
    unsigned long currentTime = millis();
    if (pin != lastPin || (currentTime - lastPressTime) > BUTTON_DEBOUNCE_MS) {
      lastPin = pin;
      lastPressTime = currentTime;
      return true;
    }
  }
  return false;
}

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();

  // Initialize buttons with pullup
  for (int i = 0; i < 4; i++) {
    pinMode(buttons[i], INPUT_PULLUP);
  }

  
  updateDisplay();
}

void loop() {
  handleButtonPress();

  delay(10);
}

void handleButtonPress() {
  for (int i = 0; i < 4; i++) {
    int reading = digitalRead(buttons[i]);
    
    // Check if button state changed
    if (reading != currentButtonState[i]) {
      // Reset debounce timer
      lastDebounceTime[i] = millis();
      currentButtonState[i] = reading;
    }
    
    // Check if enough time has passed for stable reading
    if ((millis() - lastDebounceTime[i]) > debounceDelay) {
      // If button state has changed from stable state
      if (currentButtonState[i] != lastStableState[i]) {
        
        // Detect button press (HIGH to LOW transition)
        if (lastStableState[i] == HIGH && currentButtonState[i] == LOW) {
          switch (i){
            case 0:
            Serial.println("Up");
            menuIndex--;
            if (menuIndex < 0) menuIndex = totalMenu - 1;
            Serial.println(menuIndex);
            break;

            case 1:
            Serial.println("Select");
            executeSelection();
            break;

            case 2:
            Serial.println("Down");
            menuIndex++;
            if (menuIndex >= totalMenu) menuIndex = 0;
            Serial.println(menuIndex);
            break;
            case 3:Serial.println("Back");break;
          }
          updateDisplay();
        }
        
        // Update stable state
        lastStableState[i] = currentButtonState[i];
      }
    }
  }
}

void updateDisplay(){
  switch (menuIndex){
    case 0:
      lcd.clear();
      lcd.setCursor(5, 0);
      lcd.print("Sensor");
      break;
    case 1:
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("Parameters");
      break;
    case 2:
      lcd.clear();
      lcd.setCursor(5, 0);
      lcd.print("Start");
      break;
  }
}

void executeSelection(){
  switch(menuIndex){
    case 0:
      Serial.println("Navigate To Sensor Menu");
      handleSensorMenu();
      break;
    case 1:
      Serial.println("Navigate To Parameters Menu");
      handleParamsMenu();
      break;
    case 2:
      Serial.println("Start Robot....");
      handleStart();
      break;
  }
}

void handleStart(){
  lcd.clear();
  lcd.setCursor(0, 0);
  while(true){
    lcd.setCursor(0, 0);
    lcd.print("kp: ");
    lcd.print(kp);
    lcd.print("ki: ");
    lcd.print(ki);
    lcd.setCursor(0, 1);
    lcd.print("kd: ");
    lcd.print(kd);
    if(isButtonPressed(BTN_BACK)){break;}
    
  }
}

void handleParamsMenu(){
  int paramsSelect = 0;
  int maxParamsSelect = 3;
  updateDisplayParamsMenu(paramsSelect);
  
  while(true){
    if(isButtonPressed(BTN_UP)){
      paramsSelect--;
      if(paramsSelect < 0) paramsSelect = maxParamsSelect - 1;
      Serial.println(paramsSelect);
      updateDisplayParamsMenu(paramsSelect);
    }
    if(isButtonPressed(BTN_DOWN)){
      paramsSelect++;
      if(paramsSelect >= maxParamsSelect) paramsSelect = 0;
      Serial.println(paramsSelect);
      updateDisplayParamsMenu(paramsSelect);
    }
    if(isButtonPressed(BTN_BACK)){break;}

    if(isButtonPressed(BTN_SELECT)){
      editingParams = true;
      updateDisplayParamsMenu(paramsSelect);
      handleEditParams(paramsSelect);
    }

  }
}


void handleEditParams(int selection){

  while(true){
  if(isButtonPressed(BTN_UP)){
    switch (selection){
      case 0: kp = kp + 0.1;break;
      case 1: ki = ki + 0.1;break;
      case 2: kd = kd + 0.1;break;
    }
    updateDisplayParamsMenu(selection);
  }
  if(isButtonPressed(BTN_DOWN)){
    switch (selection){
      case 0: kp = kp - 0.1;if(kp < 0) kp = 0;break;
      case 1: ki = ki - 0.1;if(ki < 0) ki = 0;break;
      case 2: kd = kd - 0.1;if(kd < 0) kd = 0;break;
    }
    updateDisplayParamsMenu(selection);
  }
  if(isButtonPressed(BTN_SELECT) || isButtonPressed(BTN_BACK)){
    editingParams = false;
    updateDisplayParamsMenu(selection);
    break;
    }
}
}


void updateDisplayParamsMenu(int selection){
  lcd.clear();
  lcd.setCursor(1, 1);

  if(editingParams == true){
    lcd.print("Editing Params");
  }else{ 
    lcd.print("Select To Edit"); 
  }

  switch (selection){
    case 0:
      lcd.setCursor(0, 0);
      lcd.print("Kp : ");
      lcd.print(kp);
      break;
    case 1:
      lcd.setCursor(0, 0);
      lcd.print("Ki : ");
      lcd.print(ki);
      break;
    case 2:
      lcd.setCursor(0, 0);
      lcd.print("Kd : ");
      lcd.print(kd);
      break;
  }
}



void handleSensorMenu(){
  lcd.clear();
  while(true){
    lcd.setCursor(0, 0);
    lcd.print("S1:0 ");
    lcd.print("S2:1 ");
    lcd.print("S3:0 ");
    if(digitalRead(BTN_BACK) == false){
      break;
    }
  }

}