// https://forum.arduino.cc/t/servo-not-respecting-code/1128506/2

#include <Servo.h>
#include <Key.h>
#include <Keypad.h>

// As it is a servo I prefer to call it myServo ... 
Servo myServo;

constexpr byte startPin = 2;
constexpr byte countPin = 9;
constexpr byte ledPin   =  4;



int blinkCounter = 0;
int newDelayMinutes = 0;
int acceptedDelayMinutes = 0;
int countDownMinutes;
unsigned long newDelayTime;
constexpr unsigned long waitTime = 1000;   // Should by 60000 ms but for test purposes just 1 sec

enum myStates  {DONOTHING, GO90, START180, WAITFOR180, WAITTOGOBACK};
myStates actState = GO90;


// To allow to handle the pins in for-loops we put their
// data in an array; this is the moste flexible way
// as the pin number do not necessarily have to be in a numerical sequence 
constexpr byte noOfPins = 6;
byte pins[noOfPins] = {A0, A1, A2, A3, A4, A5};


// As it looks that the first x pins shall be set HIGH, the rest LOW
// we can handle this in a simple for-loop
void setPins(int value) {
  for (int i = 0; i < noOfPins; i++) {
    if (i <= value - 1) {
      digitalWrite(pins[i], HIGH);
    } else {
      digitalWrite(pins[i], LOW);
    }
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(startPin,INPUT_PULLUP);
  pinMode(countPin,INPUT_PULLUP);
  pinMode(ledPin,OUTPUT);
  for (int i = 0; i < noOfPins; i++) {
    pinMode(pins[i], OUTPUT);
  }
  myServo.attach(10);
  //myServo.write(90);
  actState = GO90;
}  

void loop() { 
  handleStartButton();
  handleCountButton();
  handleBlinking();
  stateMachine();
}

void handleBlinking(){
  static unsigned long lastBlinking = 0;
  if (blinkCounter > 0) {
     if (millis()-lastBlinking > 300) {
     boolean ledState = !digitalRead(ledPin);
     lastBlinking = millis();
     digitalWrite(ledPin, ledState);
     if (!ledState) blinkCounter--;
     }
  }

}

void handleStartButton(){
  if (startButtonReleased() && newDelayMinutes > 0){
     acceptedDelayMinutes = newDelayMinutes;
     actState = START180;
     Serial.print("Sequence started with delay [min] of ");
     Serial.println(acceptedDelayMinutes);
  }
}

void handleCountButton(){
  if (countButtonReleased() && blinkCounter == 0){
     newDelayMinutes++;
     if (newDelayMinutes > 6) {
       newDelayMinutes = 1;
     }
     Serial.print("Delay value [min] set to ");
     Serial.println(newDelayMinutes);
     blinkCounter = newDelayMinutes;
  }
}

boolean startButtonReleased(){
  static unsigned long lastChangeTime = 0;
  static byte lastBstate = HIGH;
  static byte actBstate = HIGH;
  boolean bReleased = false;
  byte bState = digitalRead(startPin);
  if (bState != lastBstate){
    lastChangeTime = millis();
    lastBstate = bState;
  }
  if (actBstate != lastBstate && millis()-lastChangeTime > 30){
    actBstate = lastBstate;
    if (actBstate == HIGH) {
       bReleased = true;     
    }
  }
 return bReleased;
}

boolean countButtonReleased(){
  static unsigned long lastChangeTime = 0;
  static byte lastBstate = HIGH;
  static byte actBstate = HIGH;
  boolean bReleased = false;
  byte bState = digitalRead(countPin);
  if (bState != lastBstate){
    lastChangeTime = millis();
    lastBstate = bState;
  }
  if (actBstate != lastBstate && millis()-lastChangeTime > 30){
    actBstate = lastBstate;
    if (actBstate == HIGH) {
       bReleased = true;     
    }
  }
 return bReleased;
}



void stateMachine(){
  static unsigned long startTime = 0;
  static unsigned long countDownStartTime= 0;
  switch(actState){
     case DONOTHING :
       break;
     case GO90 :
            Serial.println("Going to 90");
            myServo.write(90);
            actState = DONOTHING;
            setPins(0);
       break;
     case START180 :
            Serial.println("Starting to WAITFOR180");
            startTime = millis();
            actState = WAITFOR180;
            countDownMinutes = acceptedDelayMinutes;
            countDownStartTime = startTime;
            setPins(acceptedDelayMinutes);
       break;  
     case WAITFOR180 :
       if (millis()-countDownStartTime >= waitTime){ 
          countDownStartTime = millis();
          countDownMinutes--;
          setPins(countDownMinutes); 
       }  
       if (millis()-startTime >= waitTime*acceptedDelayMinutes){ // Instead of delay()
          startTime = millis();
          myServo.write(180);
          actState = WAITTOGOBACK;
          Serial.println("Starting WAITTOGOBACK");
       }
       break;  
     case WAITTOGOBACK :
       if (millis()-startTime >= waitTime){ // Instead of delay()
          actState = GO90;
       }
      break;
      default:
         actState = GO90;
       break;  
  }
}