// Constants won't change. They're used here to set pin numbers:
const int BUTTON_PIN_1 = 33;    // Button pin for system 1
const int BUTTON_PIN_2 = 32;    // Button pin for system 2
const int BUTTON_PIN_3 = 35;    // Button pin for system 3

#include <Servo.h>
Servo servo1_1;   // Servo 1 for system 1 (Door Servo 1)
Servo servo1_2;   // Servo 2 for system 1 (Package Push Servo 1)  
Servo servo2_1;   // Servo 1 for system 2 (Door Servo 2)
Servo servo2_2;   // Servo 2 for system 2 (Package Push Servo 2) 
Servo servo3_1;   // Servo 1 for system 3 (Door Servo 3)
Servo servo3_2;   // Servo 2 for system 3 (Package Push Servo 3) 

int pos1_1 = 90;   // Variable to store servo1_1 position for system 1
int pos1_2 = 190;   // Variable to store servo1_2 position for system 1
int pos2_1 = 90;   // Variable to store servo2_1 position for system 2
int pos2_2 = 190;   // Variable to store servo2_2 position for system 2
int pos3_1 = 90;   // Variable to store servo3_1 position for system 3
int pos3_2 = 190;   // Variable to store servo3_2 position for system 3

bool isPressed1 = false;   // Flag variable to track button press for system 1
bool isPressed2 = false;   // Flag variable to track button press for system 2
bool isPressed3 = false;   // Flag variable to track button press for system 3

void setup() {
  // Initialize serial communication at 9600 bits per second:
  Serial.begin(9600);

  // Initialize the pushbutton pins as pull-up inputs
  // The pull-up input pins will be HIGH when the switch is open and LOW when the switch is closed.
  pinMode(BUTTON_PIN_1, INPUT_PULLUP);
  pinMode(BUTTON_PIN_2, INPUT_PULLUP);
  pinMode(BUTTON_PIN_3, INPUT_PULLUP);

  // Attach the servos to their respective pins
  servo1_1.attach(13);
  servo1_2.attach(12);
  servo2_1.attach(14);
  servo2_2.attach(27);
  servo3_1.attach(26);
  servo3_2.attach(25);
}

void loop() {
  // Read the state of the buttons
  int buttonState1 = digitalRead(BUTTON_PIN_1);
  int buttonState2 = digitalRead(BUTTON_PIN_2);
  int buttonState3 = digitalRead(BUTTON_PIN_3);

  // Print out the button states
  Serial.print("Button 1: ");
  Serial.print(buttonState1);
  Serial.print("  Button 2: ");
  Serial.print(buttonState2);
  Serial.print("  Button 3: ");
  Serial.println(buttonState3);

  // Check button state for system 1
  if (buttonState1 == LOW) {
    if (!isPressed1) {
      // Button pressed for the first time
      isPressed1 = true;
      if (pos1_1 == 90) {
        pos1_1 = 0;
        pos1_2 = 90;
        servo1_1.write(pos1_1);
        delay(3000);
        servo1_2.write(pos1_2);
        delay(15);
      } else {
        pos1_1 = 90;
        pos1_2 = 190;
        servo1_1.write(pos1_1);
        servo1_2.write(pos1_2);
        delay(15);
      }
    }
  } else {
    isPressed1 = false;  // Reset the flag when the button is released
  }

  // Check button state for system 2
  if (buttonState2 == LOW) {
    if (!isPressed2) {
      // Button pressed for the first time
      isPressed2 = true;
      if (pos2_1 == 90) {
        pos2_1 = 0;
        pos2_2 = 90;
        servo2_1.write(pos2_1);
        delay(3000);
        servo2_2.write(pos2_2);
        delay(15);
      } else {
        pos2_1 = 90;
        pos2_2 = 190;
        servo2_1.write(pos2_1);
        servo2_2.write(pos2_2);
        delay(15);
      }
    }
  } else {
    isPressed2 = false;  // Reset the flag when the button is released
  }

  // Check button state for system 3
  if (buttonState3 == LOW) {
    if (!isPressed3) {
      // Button pressed for the first time
      isPressed3 = true;
      if (pos3_1 == 90) {
        pos3_1 = 0;
        pos3_2 = 90;
        servo3_1.write(pos3_1);
        delay(3000);
        servo3_2.write(pos3_2);
        delay(15);
      } else {
        pos3_1 = 90;
        pos3_2 = 190;
        servo3_1.write(pos3_1);
        servo3_2.write(pos3_2);
        delay(15);
      }
    }
  } else {
    isPressed3 = false;  // Reset the flag when the button is released
  }
}