/*
  This sketch uses one millis() of 50mS which does the button debounce plus
  with a single counter creates several different timing periods (1, 2, 4 seconds)
  You can have as many as you want and have more counters to have several timers running 
  at the same time.
*/

#include "ESP32_Servo.h"

#define SERVO 4
#define BUTTON 32
#define LED 26

#define DEBOUNCE  50UL
#define ONE_SEC   20     //20 x 50 = 1000mS or One Second
#define TWO_SEC   40     //40 x 50 = 2000mS or Two Seconds
#define FOUR_SEC  80     //80 x 50 = 4000mS or Four Seconds
#define STOP      90
#define SLOW_CW   45
#define FAST_CW   0
#define SLOW_CCW  135
#define FAST_CCW  180

int servoSpeedSequence[]{STOP, FAST_CW, SLOW_CCW, SLOW_CW, FAST_CCW, FAST_CW};
byte seqStepTimes[]{0, TWO_SEC, ONE_SEC, FOUR_SEC, TWO_SEC, ONE_SEC};

byte timeCounter = 0, arrayIndex = 0;             //timeCounter x duty = timer periods
bool buttonPress, pressed = 0, lastPress = 1,     //for the button
     run = 0, updateServo = 0;     

uint32_t lastTime = 0;    //Creates our loop duty timer

Servo servo;

void printState(byte x){
  if(x == 5){
    Serial.println(F("Sequence Position 5 - One Second Delay"));
  }else if(x == 4){
    Serial.println(F("Sequence Position 4 - Two Seconds Delay"));
  }else if(x == 3){
    Serial.println(F("Sequence Position 3 - Four Seconds Delay"));
  }else if(x == 2){
    Serial.println(F("Sequence Position 2 - One Second Delay"));
  }else if(x == 1){
    Serial.println(F("Sequence Position 1 - Two Seconds Delay"));
  }else{
    Serial.println(F("\nStoped - Waiting for Button Press"));
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial.println(F("\n\nESP32 Servo Sequence Example"));
  pinMode(BUTTON, INPUT_PULLUP);
  pinMode(LED, OUTPUT);
  servo.attach(SERVO);
  servo.write(servoSpeedSequence[arrayIndex]);
  printState(arrayIndex);
}

void loop() {

  buttonPress = digitalRead(BUTTON);
  if(buttonPress != lastPress){
    pressed = true;
    lastTime = millis();
  }
  if(millis() - lastTime >= DEBOUNCE){
    timeCounter++;
    if(pressed){
      if(buttonPress == LOW){
        run = !run;
        digitalWrite(LED, run);
          if(run){
            arrayIndex = 1;
            timeCounter = 0;
            updateServo = true;
          }
      }
      pressed = false;
    }
    if(run){
      if(timeCounter >= seqStepTimes[arrayIndex]){
        arrayIndex++;
        timeCounter = 0;
        arrayIndex %= 6;
        updateServo = true;
          if(arrayIndex == 0){
            run = false;
            digitalWrite(LED, run);
          }
      }
    }
    if(updateServo){
      servo.write(servoSpeedSequence[arrayIndex]);
      updateServo = false;
      printState(arrayIndex);
    }
    lastTime = millis();
  }
  lastPress = buttonPress;
}