#include <LiquidCrystal.h>

const int rs = 12, en = 11, d4 = 10, d5 = 9, d6 = 8, d7 = 7;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

const int pinA = 13;
const int pinB = 4;
const int pinButton = 3;

const int motorPin = 6;
const int buzzPin = 2;
const int servoPin = 5;

int lastEncoded = 0;
int encoderValue = 0;

String menus[] = {"min speed", "max speed", "min angle", "max angle", "done"};
int menuValues[] = {3, 10, -90, 90};

int menuMaxValues[] = {100, 100};
int menuMinValues[] = {0, -100};

int numMenus = sizeof(menus) / sizeof(menus[0]);
int currentMenu = 0;

bool atMainMenu = true;

int lastClk = HIGH;
bool buttonPressed = false;
unsigned long lastButtonPressTime = 0;
const unsigned long debounceDelay = 50;

int forceUpdate = false;
int atMenu = true;

void setup() {
  lcd.begin(16, 2);
  pinMode(pinA, INPUT);
  pinMode(pinB, INPUT);
  pinMode(pinButton, INPUT);
  pinMode(motorPin, OUTPUT);
  pinMode(buzzPin, OUTPUT);

  digitalWrite(pinA, HIGH);
  digitalWrite(pinB, HIGH);
  digitalWrite(pinButton, HIGH);
  digitalWrite(motorPin, LOW);
  digitalWrite(buzzPin, LOW);

  lastEncoded = (digitalRead(pinA) << 1) | digitalRead(pinB);
  delay(100);
}

void loop() {
  if (atMenu) {
    readEncoder();
    handleButtonPress();

    if (atMainMenu) {
      displayMainMenu();
    }
    else {
      displaySetting(); 
      }}
  else {
    start(); 
    }}
  

void handleButtonPress() {
  int buttonState = digitalRead(pinButton);

  if (buttonState == LOW && !buttonPressed) {
    unsigned long currentMillis = millis();
    if (currentMillis - lastButtonPressTime >= debounceDelay) {
      buttonPressed = true;
      lastButtonPressTime = currentMillis;

      if (currentMenu == numMenus - 1 && atMainMenu) {
        saveSettings();
      } else {
        atMainMenu = !atMainMenu; // Toggle atMainMenu
        forceUpdate = true;
        lcd.clear();
      }}
  } else if (buttonState == HIGH) {
    buttonPressed = false; }}

void readEncoder() {
  int newClk = digitalRead(pinA);
  if (newClk != lastClk) {
    lastClk = newClk;
    int dtValue = digitalRead(pinB);
    if (newClk == LOW && dtValue == HIGH) {
      leftEncoder();
    }
    if (newClk == LOW && dtValue == LOW) {
      rightEncoder(); }}}

void rightEncoder() {
  if (atMainMenu) {
    last_menu();
  }
  else {
    menuValues[currentMenu]--;
    if (menuValues[currentMenu] < menuMinValues[currentMenu]) {
      menuValues[currentMenu] = menuMaxValues[currentMenu]; }}}

void leftEncoder() {
  if (atMainMenu) {
    next_menu();
  }
  else {
    menuValues[currentMenu]++;
    if (menuValues[currentMenu] > menuMaxValues[currentMenu]) {
      menuValues[currentMenu] = menuMinValues[currentMenu]; }}}


void last_menu() {
  currentMenu = (currentMenu + 1) % numMenus;
}

void next_menu() {
  currentMenu = (currentMenu + numMenus - 1) % numMenus;
}

void displayMainMenu() {
  static int prevMenuIndex = -1; // Track the previous menu index

  // Check if the menu index has changed
  if (currentMenu != prevMenuIndex || forceUpdate) {
    lcd.clear();

    // Display the current menu item on the first line
    lcd.setCursor(0, 0);
    lcd.print("> ");
    lcd.print(menus[currentMenu]);

    // Display the next menu item on the second line if it exists
    if (currentMenu < numMenus - 1) {
      lcd.setCursor(0, 1);
      lcd.print("  ");
      lcd.print(menus[currentMenu + 1]);
    }

    forceUpdate = false;

    // Update the previous menu index
    prevMenuIndex = currentMenu;
  }}

void displaySetting() {
  static int prevValue = -1; // Track the previous setting value

  // Check if the setting value has changed
  if (menuValues[currentMenu] != prevValue || forceUpdate) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Setting: ");
    lcd.print(menus[currentMenu]);

    lcd.setCursor(0, 1);
    lcd.print("Value: ");
    lcd.print(menuValues[currentMenu]);

    forceUpdate = false;

    // Update the previous setting value
    prevValue = menuValues[currentMenu];
  }}

void saveSettings() {
  // Placeholder for saving logic
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Settings Saved!");
  delay(2000); // Show confirmation for 2 seconds
  lcd.clear();
//  atMainMenu = true; // Return to main menu
//  forceUpdate = true; // Force update to refresh the menu display
  atMenu = false;
}




void start () {
  digitalWrite(motorPin, HIGH);
  delay(1000);
  digitalWrite(buzzPin, HIGH);
  delay(50);
  digitalWrite(buzzPin, LOW);
  digitalWrite(motorPin, LOW);
}