#include <Servo.h>

// Create servo objects for each servo
Servo servo_1;
Servo servo_2;

// Define pin numbers for buttons
const int A_clock = 12;        // Button 1 for Servo 1 (90°)
const int A_antiClock = 11;    // Button 2 for Servo 1 (180°)
const int B_clock = 10;        // Button 3 for Servo 2 (90°)
const int B_antiClock = 9;     // Button 4 for Servo 2 (180°)

// Variables to store button states
int a_clock_push, a_anticlock_push, b_clock_push, b_anticlock_push;

void setup() {
  // Initialize serial communication
  Serial.begin(115200);

  // Attach servos to pins
  servo_1.attach(3);
  servo_2.attach(5);

  // Initialize button pins as inputs
  pinMode(A_clock, INPUT);
  pinMode(A_antiClock, INPUT);
  pinMode(B_clock, INPUT);
  pinMode(B_antiClock, INPUT);

  // Set initial position of servos to 0°
  servo_1.write(0);
  servo_2.write(0);

  Serial.println("Setup complete.");
}

void loop() {
  // Read the state of all buttons
  a_clock_push = digitalRead(A_clock);
  a_anticlock_push = digitalRead(A_antiClock);
  b_clock_push = digitalRead(B_clock);
  b_anticlock_push = digitalRead(B_antiClock);

  // Debugging: Print button states
  Serial.print("Button 1 state: ");
  Serial.println(a_clock_push);
  Serial.print("Button 2 state: ");
  Serial.println(a_anticlock_push);
  Serial.print("Button 3 state: ");
  Serial.println(b_clock_push);
  Serial.print("Button 4 state: ");
  Serial.println(b_anticlock_push);

  // Control Servo 1
  if (a_clock_push == HIGH) {          // Button 1 pressed
    servo_1.write(90);                 // Move Servo 1 to 90°
    Serial.println("Servo 1: 90°");
  } else if (a_anticlock_push == HIGH) { // Button 2 pressed
    servo_1.write(180);                // Move Servo 1 to 180°
    Serial.println("Servo 1: 180°");
  } else {
    servo_1.write(0);                  // Default position (0°)
    Serial.println("Servo 1: 0°");
  }

  // Control Servo 2
  if (b_clock_push == HIGH) {          // Button 3 pressed
    servo_2.write(90);                 // Move Servo 2 to 90°
    Serial.println("Servo 2: 90°");
  } else if (b_anticlock_push == HIGH) { // Button 4 pressed
    servo_2.write(180);                // Move Servo 2 to 180°
    Serial.println("Servo 2: 180°");
  } else {
    servo_2.write(0);                  // Default position (0°)
    Serial.println("Servo 2: 0°");
  }

  // Small delay to debounce buttons
  delay(100);
}