#include <Servo.h>

const int PushButton = 8;   // extra Button
int PushButtonState = 0;   // extra Button
int PushButtonLastState = 0;   // extra Button
uint8_t PushButtonPressed = 0;   // extra Button
unsigned long previousMillisPushButton = 0;   // extra Button

Servo myServo;

unsigned long MOVING_TIME;   // DegreeXXX x MsDelayXXX
unsigned long moveStartTime;
unsigned long MillisDelay;

unsigned long debounceDelay = 250;    //   Dynamic*
unsigned long nextStepDelay = 2000;   //   Dynamic*
unsigned long bounceDelay = 250;     //   Dynamic *


unsigned long ShakeDelay = 100;       //   Dynamic    Delay between +Degree and -Degree from current position

int ShakeDegrees = 2;   // Dynamic   +- Degrees from current position

int Degree1st = 150;   // Dynamic 
int Degree2nd = 120;    // Dynamic
int Degree3rd = 148;    // Dynamic
// Degree4th Not Dynamic --- Depends on current position
int Degree5th = 120;    // Dynamic
int Degree6th = 150;   // Dynamic

int MsDelay1st = 7;   // Dynamic   Delay 0 to 150
int MsDelay2nd = 100;   // Dynamic  Delay 150 to 120
int MsDelay3rd = 100;  // Dynamic  Delay between 120 and 148
int MsDelay4th = 100;  // Dynamic  Delay paused shake position to 120
int MsDelay5th = 100;   // Dynamic  Delay 120 to 150
int MsDelay6th = 7;   // Dynamic  Delay 150 to 0


int pressCount = 0;
int Delay = 0;
// int pressDelay = 0;
int Thesh3 = 0;
int Thesh4 = 0;
int currentPos = 0;
int Pos = 0;
int Enable = 0;

void setup() {

  pinMode(PushButton, INPUT_PULLUP);   // extra Button
  
  Serial.begin(9600);
  myServo.attach(9);
  myServo.write(0);
  
  delay(1000);
}

void loop() {

  unsigned long progress = millis() - moveStartTime;

  PushButtonState = digitalRead(PushButton);   // extra Button

  if (PushButtonState == LOW and PushButtonLastState == 0 and PushButtonPressed == 0){   // extra Button
    previousMillisPushButton = millis();
    PushButtonPressed = 1;
  }

  if (PushButtonState == HIGH and PushButtonLastState == 0 and PushButtonPressed == 1){   // extra Button
    PushButtonPressed = 0;
  }
    
  if (PushButtonState == LOW and PushButtonLastState == 0 and PushButtonPressed == 1){   // extra Button
    if (millis() - previousMillisPushButton >= bounceDelay){ 
      PushButtonPressed = 2;
      PushButtonLastState = 1;
    }
  }

  if (PushButtonState == HIGH and PushButtonLastState == 1 and PushButtonPressed == 2){   // extra Button
    PushButtonLastState = 0;
    PushButtonPressed = 0;
    Enable = 1;
    pressCount++;
    moveStartTime = millis();
    debounceDelay = millis();
  }

  if (pressCount == 3 or pressCount == 4 or pressCount == 5){
    if ((millis() - debounceDelay) >= nextStepDelay){
      Enable = 0;
    }
  }

  if (pressCount == 1) { // 0-150
    MOVING_TIME = (Degree1st * MsDelay1st);
    if (progress <= MOVING_TIME) {
      long angle = map(progress, 0, MOVING_TIME, 0, Degree1st);
      myServo.write(angle);
    }
    if (myServo.read() == Degree1st){
      Enable = 0; 
    }
  }

  if (pressCount == 2) { // 150-110
    MOVING_TIME = ((Degree1st - Degree2nd) * MsDelay2nd);
    if (progress <= MOVING_TIME) {
      long angle = map(progress, 0, MOVING_TIME, Degree1st, Degree2nd);
      myServo.write(angle);
    }
    if (myServo.read() == Degree2nd){
      Enable = 0; 
    }
  }

  if (pressCount == 3) { //  110-145
    MOVING_TIME = ((Degree3rd - Degree2nd) * MsDelay3rd);
    if (Thesh3 == 0 and progress <= MOVING_TIME) {
      long angle = map(progress, 0, MOVING_TIME, Degree2nd, Degree3rd);
      myServo.write(angle);
    }
    if (Thesh3 == 0 and myServo.read() == Degree3rd and Delay == 0){
      MillisDelay = millis();
      Delay = 1;
    }
    if (Thesh3 == 0 and myServo.read() == Degree3rd and Delay == 1){
      if (millis() - MillisDelay >= 1999) {
        Delay = 0;
        Thesh3 = 1;
        moveStartTime = millis();
      }
    }
    if (Thesh3 == 1 and progress <= MOVING_TIME) {
      long angle = map(progress, 0, MOVING_TIME, Degree3rd, Degree2nd);
      myServo.write(angle);
    }
    if (Thesh3 == 1 and myServo.read() == Degree2nd and Delay == 0){
      MillisDelay = millis();
      Delay = 1;
    }
    if (Thesh3 == 1 and myServo.read() == Degree2nd and Delay == 1){
      if (millis() - MillisDelay >= 1999) {
        Delay = 0;
        Thesh3 = 0;
        moveStartTime = millis();
      }
    }
  }
  if (pressCount == 4) { //  Shake
    if (currentPos == 0){
      Pos = myServo.read();
      currentPos =1;
    }
    if (Thesh4 == 0) {
      myServo.write(Pos+ShakeDegrees);
    }
    if (Thesh4 == 0 and Delay == 0){
      MillisDelay = millis();
      Delay = 1;
    }
    if (Thesh4 == 0 and Delay == 1){
      if (millis() - MillisDelay >= ShakeDelay) {
        Delay = 0;
        Thesh4 = 1;
        moveStartTime = millis();
      }
    }
    if (Thesh4 == 1) {
      myServo.write(Pos-ShakeDegrees);
    }
    if (Thesh4 == 1 and Delay == 0){
      MillisDelay = millis();
      Delay = 1;
    }
    if (Thesh4 == 1 and Delay == 1){
      if (millis() - MillisDelay >= ShakeDelay) {
        Delay = 0;
        Thesh4 = 0;
        moveStartTime = millis();
      }
    }
  }
  if (pressCount == 5){ // Stop Shake, Preparation MOVING_TIME
    Pos = myServo.read();
    if (Pos << Degree5th) {
      MOVING_TIME = ((Degree5th - Pos) * MsDelay4th);
    }
    if (Pos >> Degree5th) {
      MOVING_TIME = ((Pos - Degree5th ) * MsDelay5th);
    }
    if (Pos == Degree5th) {
      MOVING_TIME = 0;
    }
  }
  if (pressCount == 6) { //  Shake-110
    if (progress <= MOVING_TIME) {
      long angle = map(progress, 0, MOVING_TIME, Pos, Degree5th);
      myServo.write(angle);
    }
    if (myServo.read() == Degree5th){
      Enable = 0; 
    }
  }
  if (pressCount == 7) { //  110-150
    MOVING_TIME = ((Degree6th - Degree5th) * MsDelay5th);
    if (progress <= MOVING_TIME) {
      long angle = map(progress, 0, MOVING_TIME, Degree5th, Degree6th);
      myServo.write(angle);
    }
    if (myServo.read() == Degree6th){
      Enable = 0; 
    }
  }
  if (pressCount == 8) { //  150-0
    MOVING_TIME = (Degree6th * MsDelay6th);
    if (progress <= MOVING_TIME) {
      long angle = map(progress, 0, MOVING_TIME, Degree6th, 0);
      myServo.write(angle);
    }
    if (myServo.read() == 0) {
      Enable = 0;
      pressCount = 0;
      Delay = 0;
      Thesh3 = 0;
      Thesh4 = 0;
      currentPos = 0;
      Pos = 0;
      MOVING_TIME = 0;
    }
  }
}