// 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 buttonPin = 11;


const byte ROWS = 4;
const byte COLS = 3;
char keys[ROWS][COLS] = {
  {'1', '2', '3'},
  {'4', '5', '6'},
  {'7', '8', '9'},
  {'*', '0', '#'}
};
byte rowPins[ROWS] = {2, 3, 4, 5};
byte colPins[COLS] = {6, 7, 8};
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );


boolean setSequenceActive = false;
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 handleKeypad() {
    char key = keypad.getKey();
    if (key == '*') {
         Serial.println("Start sequence");
         actState = START180;
    }
    if (key >= '1' && key <= '6') {
      newDelayMinutes = key - '0';
      Serial.print("New delay value ");
      Serial.print(newDelayMinutes);
      Serial.println("\ min");
    }
    if (key == '#'){
      if (newDelayMinutes > 0) { // start sequence only if the acceptedDelayMinutes are greater than zero
         acceptedDelayMinutes = newDelayMinutes;
         Serial.print("Accepted delay value ");
         Serial.print(acceptedDelayMinutes);
         Serial.println(" min");
      }
    }
}

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

void loop() { 
  handleKeypad();
  stateMachine();
}

boolean buttonReleased(){
  static unsigned long lastChangeTime = 0;
  static byte lastBstate = HIGH;
  static byte actBstate = HIGH;
  boolean bReleased = false;
  byte bState = digitalRead(buttonPin);
  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 :
            if (buttonReleased() && acceptedDelayMinutes > 0) {
               actState = START180;
            }
       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;  
  }
}

/*
  myServo.write(90);
  setPins(val);
  delay(val * waitTime);
  myServo.write(180);
  delay(waitTime);
*/