#include <Servo.h>

const int INTERVAL = 500;

const int servoPin1 = 11;
const int servoPin2 = 12;
const int servoPin3 = 13;
const int servoPin4 = 10;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

// State variables
long currentCommand = 0;
unsigned long previousMillis = 0;
bool shouldUpdate1and2 = false;


void executeCommand(int command, unsigned long currentMillis) {
  switch (command) {
    case 1:
      Serial.println("Command 1: Move Servos");
      servo3.write(180);
      servo4.write(180);
      previousMillis = currentMillis;
      shouldUpdate1and2 = true;
      break;
    case 2:
      Serial.println("Command 2: Reset Servos");
      servo1.write(90);
      servo2.write(90);
      servo3.write(90);
      servo4.write(90);
    default:
      currentCommand = 0;
      break;
  };
}

void updateServos1and2(unsigned long currentMillis) {
  if(shouldUpdate1and2 && currentMillis - previousMillis >= INTERVAL) {
    servo1.write(180);
    servo2.write(180);
    currentCommand = 0;
    shouldUpdate1and2 = false;
  }
} 

void setup() {
    // put your setup code here, to run once:
  Serial.begin(9600);

  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);

  while(!Serial) {}
  Serial.println("Starting");
  currentCommand = 0;
  shouldUpdate1and2 = false;
}

void loop() {
  unsigned long current = millis();

  // This will ignore additional commands on Serial until a command
  // completes and resets currentCommand to 0.
  if( currentCommand == 0 && Serial.available() ) {
    currentCommand = Serial.parseInt();
    executeCommand(currentCommand, current);
  }

  updateServos1and2(current);
}