#include <Servo.h>
// Main Servos 
 Servo servo1;
 Servo servo2;
 Servo servo3;
 Servo servo4;
 Servo servo5;

// Special Servos 
 Servo servo6;
 Servo servo7;  // inverted, pin 2 
 Servo servo9;
 Servo servo10;  // inverted, pin 11

// Pins 
 const int servo1Pin = 3;
 const int servo2Pin = 5;
 const int servo3Pin = 6;
 const int servo4Pin = 9;
 const int servo5Pin = 10;
 const int servo6Pin = A0;
 const int servo7Pin = 2;  // was A1 
 const int servo9Pin = A3;
 const int servo10Pin = 11;  // it was A4

// Buttons 
 const int button1Pin = 7;
 const int button2Pin = 8;
 const int button3Pin = 12;
 const int button4Pin = 4;  // Special sequence

// States 
 bool stateServo2 = false;
 bool stateServo3 = false;
 bool stateServo4 = false;
 bool stateSpecial = false;


// Previous states 
 bool lastStateButton1 = HIGH;
 bool lastStateButton2 = HIGH;
 bool lastStateButton3 = HIGH;
 bool lastStateButton4 = HIGH;

void servowrite(int s,int p){
  Serial.print("Servo ");Serial.print(s); 
  Serial.print(" -> ");Serial.println(p);
}

void servo1write(int p) {
  servowrite (1,p);  
  //servo1.write(p);    
}

void servo2write(int p) {
  servowrite (2,p)  ;   
  //servo2.write(p);    
}

void servo3write(int p) {
  servowrite (3,p);
  //servo3.write(p);      
}

void servo4write(int p) {
  servowrite (4,p);
  //servo4.write(p);      
}

void servo5write(int p) {
  servowrite (5,p);
  //servo5.write(p);    
}

void servo6write(int p) {
  servowrite (6,p);
  //servo6.write(p);     
}

void servo7write(int p) {
  servowrite (7,p); 
  //servo7.write(p);     
}

void servo9write(int p) {
  servowrite (9,p);
  //servo9.write(p);      
}

void servo10write(int p) {
  servowrite (10,p);
  //servo10.write(p);      
}


void setup() { 
    Serial.begin(115200);
    delay(500);
    Serial.println("Setup");
    servo1.attach(servo1Pin);
    servo2.attach(servo2Pin);
    servo3.attach(servo3Pin);
    servo4.attach(servo4Pin);
    servo5.attach(servo5Pin);

    servo6.attach(servo6Pin);
    servo7.attach(servo7Pin);
    servo9.attach(servo9Pin);
    servo10.attach(servo10Pin);


  // Starting positions 
    servo1write(180);
    servo2write(0);
    servo3write(0);
    servo4write(0);
    servo5write(0);
  
    servo6write(0);
    servo7write(90);  // inverted 
    servo9write(0);
    servo10write(90);  // inverted

    pinMode(button1Pin, INPUT_PULLUP);
    pinMode(button2Pin, INPUT_PULLUP);
    pinMode(button3Pin, INPUT_PULLUP);
    pinMode(button4Pin, INPUT_PULLUP);

   Serial.println("Setup complete");

 }

void loop() { 
  //Serial.println("Loop");

  bool readingButton1 = digitalRead(button1Pin);
  bool readingButton2 = digitalRead(button2Pin);
  bool readingButton3 = digitalRead(button3Pin);
  bool readingButton4 = digitalRead(button4Pin);

  // Button 1: Servo1 and Servo2 
  if (readingButton1 == LOW && lastStateButton1 == HIGH) { 
  Serial.println("Button 1 Pressed "  ) ;
  stateServo2 = !stateServo2;
  servo1write(stateServo2 ? 90 : 180);
  servo2write(stateServo2 ? 90 : 0);
  }

  // Button 2: Servo3 and Servo5 
  if (readingButton2 == LOW && lastStateButton2 == HIGH) { 
  Serial.println("Button 2 Pressed "  ) ;
  stateServo3 = !stateServo3;
  servo3write(stateServo3 ? 50 : 0);
  servo5write(stateServo3 ? 90 : 0);
  }

  // Button 3: Servo4 
  if (readingButton3 == LOW && lastStateButton3 == HIGH) { 
  Serial.println("Button 3 Pressed "  ) ;
  stateServo4 = !stateServo4;
  servo4write(stateServo4 ? 90 : 0);
  }

  // Button 4: Special sequence (Servo6,7,9,10) 
  if (readingButton4 == LOW && lastStateButton4 == HIGH) { 
    Serial.println("Button 4 pressed");
    stateSpecial = !stateSpecial;
    if (stateSpecial) {
      // OPEN
      Serial.println("Open");
      servo6write(90);
      servo7write(0);   // inverted
      delay(250);
      servo9write(90);
      servo10write(0);  // inverted

    } else {
      // TO CLOSE
      Serial.println("Close");
      servo6write(0);
      servo7write(90);  // inverted
      delay(250);
      servo9write(0);
      servo10write(90);  // inverted
    }

  }

  lastStateButton1 = readingButton1;
  lastStateButton2 = readingButton2;
  lastStateButton3 = readingButton3;
  lastStateButton4 = readingButton4;
  delay(50);  // debounce 

}