#include <LiquidCrystal_I2C.h>
#include <Servo.h>

// LCD and Rotary Encoder Settings
#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2
#define CLK 6
#define DT 5
#define SW 4

// Servo and Motor Shield Settings
#define SERVO_PIN 10
#define MOTOR_SPEED_PIN 3
#define MOTOR_DIRECTION_PIN 12

// Menu Settings
String menuItems[] = {"Speed", "Angle", "Standard"};
int menuValues[] = {50, 90, 0}; // Default values: Speed, Angle
int menuSize = sizeof(menuItems) / sizeof(menuItems[0]);

int currentStateCLK;
int lastStateCLK;
int currentStateSW;
int lastStateSW = HIGH;
int menuIndex = 0;
bool editMode = false;

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);
Servo angleServo;

void setup() {
  // Init LCD
  lcd.init();
  lcd.backlight();
  displayMenu();

  // Setup Rotary Encoder
  pinMode(CLK, INPUT);
  pinMode(DT, INPUT);
  pinMode(SW, INPUT_PULLUP);
  lastStateCLK = digitalRead(CLK);
  lastStateSW = digitalRead(SW);

  // Setup Servo and Motor Shield
  angleServo.attach(SERVO_PIN);
  pinMode(MOTOR_SPEED_PIN, OUTPUT);
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);
}

void loop() {
  currentStateCLK = digitalRead(CLK);
  currentStateSW = digitalRead(SW);

  // Check for rotation
  if (currentStateCLK != lastStateCLK && currentStateCLK == 1) {
    if (!editMode) {
      // Navigate through menu
      if (digitalRead(DT) != currentStateCLK) {
        menuIndex++;
      } 
      else {
        menuIndex--;
      }
      menuIndex = (menuIndex + menuSize) % menuSize;
      displayMenu();
    } 
    else {
      // Adjust the selected setting
      if (digitalRead(DT) != currentStateCLK) {
        menuValues[menuIndex]++;
      } 
      else {
        menuValues[menuIndex]--;
      }
      applySettings();
      displayEditValue();
    }
  }

  // Check if the button is pressed
  if (currentStateSW == LOW && lastStateSW == HIGH) {
    if (editMode) {
      // Exit edit mode
      editMode = false;
      displayMenu();
    } 
    else {
      // Enter edit mode
      editMode = true;
      displayEditValue();
    }
    delay(300); // Debounce delay
  }

  lastStateCLK = currentStateCLK;
  lastStateSW = currentStateSW;
}

void displayMenu() {
  lcd.clear();
  int start = menuIndex - LCD_LINES / 2;
  start = max(0, min(start, menuSize - LCD_LINES));
  for (int i = 0; i < LCD_LINES; i++) {
    int itemIndex = start + i;
    if (itemIndex < menuSize) {
      lcd.setCursor(1, i);
      if (itemIndex == menuIndex) {
        lcd.print(">");
        lcd.setCursor(2, i);
      } 
      else {
        lcd.setCursor(2, i);
      }
      lcd.print(menuItems[itemIndex]);
    }
  }
}

void displayEditValue() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(menuItems[menuIndex]);
  lcd.setCursor(0, 1);
  lcd.print("Value: ");
  lcd.print(menuValues[menuIndex]);
}

void applySettings() {
  // Apply settings based on menu selection
  if (menuIndex == 0) { // Speed
    int speedValue = menuValues[0]; // Adjust this logic based on your motor and setup
    analogWrite(MOTOR_SPEED_PIN, speedValue);
  } 
  else if (menuIndex == 1) { // Angle
    int angleValue = menuValues[1];
    angleServo.write(angleValue);
  } 
  else if (menuIndex == 2) { // Standard Settings
    menuValues[0] = 50; // Default Speed
    menuValues[1] = 90; // Default Angle
    applySettings(); // Apply default settings
  }
}