#include <Servo.h>

const byte encoderPinA = 2;
const byte encoderPinB = 3;
const byte buttonPin = 4;
const byte servoPin = 9;

Servo myservo;
int position[3] = {90, 45, 135}; // default servo positions
int delayTime[3] = {1000, 500, 2000}; // default delay times
int currentPosition = 0;
int encoderA;
int encoderB;

int encoderValue = 0;
int lastEncoderA = LOW;
int buttonState = LOW;
long buttonPressTime = 0;

void setup() {
  myservo.attach(servoPin);
  pinMode(encoderPinA, INPUT_PULLUP);
  pinMode(encoderPinB, INPUT_PULLUP);
  pinMode(buttonPin, INPUT_PULLUP);
  attachInterrupt(0, updateEncoder, CHANGE);
  Serial.begin(9600);
}

void loop() {
  buttonState = !digitalRead(buttonPin);
  Serial.print(encoderValue);
  Serial.print("\r");

  // check for button press
  if (buttonState == HIGH && millis() - buttonPressTime > 50) {
    buttonPressTime = millis();
    if (millis() - buttonPressTime < 3000) {
      // setting mode for delay time
      delaySettingMode();
    } else if (millis() - buttonPressTime > 5000) {
      // setting mode for angle
      angleSettingMode();
    }
  }

  // check for encoder rotation
  if (encoderA != lastEncoderA) {
    if (encoderA == LOW) {
      if (encoderB == LOW) {
        encoderValue--;
      } else {
        encoderValue++;
      }
    }
    lastEncoderA = encoderA;
  }

  // execute cycle of positions and delays
  for (int i = 0; i < 3; i++) {
    myservo.write(position[i]);
    delay(delayTime[i]);
  }
}

void updateEncoder() {
  int encoderValueA = digitalRead(encoderPinA);
  int encoderValueB = !digitalRead(encoderPinB);
  if (encoderValueA != encoderValueB) {
    if (encoderValueA == HIGH) {
      encoderValue++;
    } else {
      encoderValue--;
    }
  }
}



void delaySettingMode() {
  // enter setting mode for delay times
  for (int i = 0; i < 3; i++) {
    while (true) {
      buttonState = !digitalRead(buttonPin);
      if (buttonState == HIGH && millis() - buttonPressTime > 50) {
        buttonPressTime = millis();
        delayTime[i] += encoderValue * 100;
        encoderValue = 0;
        break;
      }
    }
  }
}

void angleSettingMode() {
  // enter setting mode for angles
  for (int i = 0; i < 3; i++) {
    while (true) {
      buttonState = digitalRead(buttonPin);
      if (buttonState == HIGH && millis() - buttonPressTime > 50) {
        buttonPressTime = millis();
        position[i] += encoderValue;
        encoderValue = 0;
        break;
      }
    }
  }
}