#include<Servo.h>
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

int value, value2 ;
int ledpin = 10;                           // light connected to digital pin 10
int ledpin2 = 11;                           // light connected to digital pin 11
long time = 0;

int periode = 30000;


byte stateServo = 0;
byte stateStep = 0;
unsigned long lastDelay = 0;
byte intervalDelay;
byte currentStep = 0;
byte finalStep = 180;


void setup() {
  servo1.attach(9);
  servo2.attach(8);
  servo3.attach(7);
  servo4.attach(6);
  // put your setup code here, to run once:

}


void loop() {
  
    time = millis();
    value = 128 + 127 * cos(2 * PI / periode * time);
    value2 = 128 + 127 * cos(2 * PI / periode * time);
    analogWrite(ledpin, value);           // sets the value (range from 0 to 255)
    analogWrite(ledpin2, value2);           // sets the value (range from 0 to 255)
  

  switch (stateServo) {
    case 0:
      switch (stateStep) {
        case 0: servo1.write(currentStep); stateStep++; nextDelay(50); break;
        case 1: if (stepDelay()) {
            servo2.write(currentStep);
            stateStep++;
            nextDelay(30);
          } break;
        case 2: if (stepDelay()) {
            servo3.write(currentStep);
            stateStep++;
            nextDelay(40);
          } break;
        case 3: if (stepDelay()) {
            servo4.write(currentStep);
            stateStep++;
            nextDelay(60);
          } break;
        case 4: if (stepDelay()) {
            currentStep++;
            if (currentStep == finalStep) {
              stateServo++; stateStep = 0; currentStep = 180; finalStep = 0;
            } else stateStep = 0;
          } break;

      }
      break;
    case 1:
      switch (stateStep) {
        case 0: servo1.write(currentStep); stateStep++; nextDelay(50); break;
        case 1: if (stepDelay()) {
            servo2.write(currentStep);
            stateStep++;
            nextDelay(30);
          } break;
        case 2: if (stepDelay()) {
            servo3.write(currentStep);
            stateStep++;
            nextDelay(40);
          } break;
        case 3: if (stepDelay()) {
            servo4.write(currentStep);
            stateStep++;
            nextDelay(60);
          } break;
        case 4: if (stepDelay()) {
            currentStep--;
            if (currentStep == finalStep) {
              stateServo++;
              stateStep = 0;
              currentStep = 0;
              finalStep = 20;
            } else stateStep = 0;
          }
          break;
      }
      break;

    case 2:
      switch (stateStep) {
        case 0: servo1.write(currentStep); servo3.write(currentStep); stateStep++; nextDelay(10); break;
        case 1: if (stepDelay()) {
            currentStep++;
            if (currentStep == finalStep) {
              stateServo++;
              stateStep = 0;
              currentStep = 20;
              finalStep = 0;
            } else stateStep = 0;
          }
          break;
      }
      break;
    case 3:
      switch (stateStep) {
        case 0: servo1.write(currentStep); servo3.write(currentStep); stateStep++; nextDelay(10); break;
        case 1: if (stepDelay()) {
            currentStep--;
            if (currentStep == finalStep) {
              stateServo = 0;
              stateStep = 0;
              currentStep = 0;
              finalStep = 180;
            } else stateStep = 0;
          }
          break;
      }
      break;
  }
}

void nextDelay(byte d) {
  intervalDelay = d;
  lastDelay = millis();
}

bool stepDelay() {
  bool result = false;
  if (millis() - lastDelay >= intervalDelay) {
    result = true;
  }
  return result;
}