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

Servo myServo;
LiquidCrystal_I2C lcd(0x27, 20, 4); // Adjust the address (0x27) if needed

// Define the pins for the buttons and servo
const int button1Pin = 2;
const int button2Pin = 3;
const int servoPin = 9;

// Servo positions array (angles corresponding to B1, B2, B3, B4, B5)
int servoPositions[] = {0, 45, 90, 135, 180}; // 5 predefined positions
int currentPositionIndex = 0;
int servoAngle = 0; // Angle controlled by Button 2

// Button state variables
int button1State = 0;
int button2State = 0;
int lastButton1State = 0;
int lastButton2State = 0;

void setup() {
  // Set up the servo and LCD
  myServo.attach(servoPin);
  lcd.init();
  lcd.backlight();

  // Initialize buttons as input
  pinMode(button1Pin, INPUT_PULLUP);
  pinMode(button2Pin, INPUT_PULLUP);

  // Set the servo to the initial position and display it
  myServo.write(servoPositions[currentPositionIndex]);
  updateLCD();
}

void loop() {
  // Read button states
  button1State = digitalRead(button1Pin);
  button2State = digitalRead(button2Pin);

  // Check if button1 is pressed to display the current button function (B1 to B5)
  if (button1State == LOW && lastButton1State == HIGH) {
    // Display the button function (current B1, B2, etc.)
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Button Function: ");
    lcd.setCursor(0, 1);
    
    // Display current button function based on the servo position
    switch (currentPositionIndex) {
      case 0:
        lcd.print("B1: 0 deg");
        break;
      case 1:
        lcd.print("B2: 45 deg");
        break;
      case 2:
        lcd.print("B3: 90 deg");
        break;
      case 3:
        lcd.print("B4: 135 deg");
        break;
      case 4:
        lcd.print("B5: 180 deg");
        break;
    }
    delay(200); // Debounce delay
  }

  // Check if button2 is pressed to move to the next servo position (angle)
  if (button2State == LOW && lastButton2State == HIGH) {
    currentPositionIndex++;
    if (currentPositionIndex >= 5) { // Wrap back to the start if beyond last position
      currentPositionIndex = 0;
    }
    myServo.write(servoPositions[currentPositionIndex]);
    updateLCD();
    delay(200); // Debounce delay
  }

  // Update last button states
  lastButton1State = button1State;
  lastButton2State = button2State;
}

// Function to update LCD with the current servo position and button labels
void updateLCD() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Servo Position: ");
  lcd.setCursor(0, 1);
  lcd.print(servoPositions[currentPositionIndex]);

  // Display button functions
  lcd.setCursor(0, 2);
  lcd.print("Press B1 for Func");
  lcd.setCursor(0, 3);
  lcd.print("Press B2 for Move");
}