#include <Servo.h>
#include <OneButton.h>

Servo servo1;
Servo servo2;
int switch_state;
int servos_state = 0; //Int used to reference state of both servos at same time
int servo1_state = 0; // Int used to reference state of just servo 1
int servo2_state = 0; // Int used to reference state of just servo 2
const int button_pin = 2;
const int switch_pin = 4; // switch outlet 
const int servo1_pin = 8; // the output number for the servo drive №1
const int servo2_pin = 9; // the output number for the servo drive №2
const int servo_angle = 35; // the angle at which the servo will turn
OneButton button(button_pin);
void setup()
{
  pinMode(switch_pin, INPUT);
  Serial.begin(115200);
  servo1.attach(servo1_pin);
  servo2.attach(servo2_pin);
  button.setDebounceTicks(50);
  button.attachClick(ClickFunction);
  button.attachClick(ClickFunction2);
  button.attachDoubleClick(DoubleClickFunction);
  button.attachDoubleClick(DoubleClickFunction2);
  servo1.write(0);
  servo2.write(0);
}

void loop()
{
button.tick();
  switch_state = digitalRead(switch_pin);
if (switch_state == HIGH)
    {   
    if (servos_state == 0) {
        Serial.print(" Closing ");
        servo1.write(servo_angle);
        servo2.write(servo_angle);
        button.attachClick(ClickFunction2);}
    if (servos_state == 1) {
        Serial.print(" Opening ");
        servo1.write(-servo_angle);
        servo2.write(-servo_angle);
        button.attachClick(ClickFunction);}
      }
else {
  if (servo1_state == 1) {
      Serial.print(" Closing Servo 1 ");
      servo1.write(-servo_angle);
      button.attachClick(ClickFunction); }
  if (servo1_state == 0) {
      Serial.print(" Opening Servo 1 ");
      servo1.write(servo_angle);
      button.attachClick(ClickFunction2); }
  if (servo2_state == 1) {
      Serial.print(" Closing Servo 2 ");
      servo2.write(-servo_angle);
      button.attachDoubleClick(DoubleClickFunction); }
  if (servo2_state == 0) {
      Serial.print(" Opening Servo 2 ");
      servo2.write(servo_angle);
      button.attachDoubleClick(DoubleClickFunction2); }
     }
}

void ClickFunction() {
        int (servos_state = 0);
        int (servo1_state = 0);
}
void ClickFunction2() {
        int (servos_state = 1);
        int (servo1_state = 1);
}
void DoubleClickFunction() {
        int (servo2_state = 0);
}
void DoubleClickFunction2() {
        int (servo2_state = 1);
}