//1+2+3+4+5a+6a
int pinBtn = 10;
int pinRed = 13;
int pinYellow = 12;
int pinGreen = 11;
int state = 0; //0 - STOP, 1 - PREPARE_TO_GO, 2 - GO, 3 - PREPARE_TO_STOP
unsigned long timestamp;
int timeStop = 5000;
int timePrepareToGo= 2000;
int timeGo= 5000;
int timePrepareToStop = 2000;

const int A = 4, B = 9, C = 3, D = 5, E = 7, F = 6, G = 8;
int pinsDigit[] = {A,B,C,D,E,F,G};

void setup(){
  pinMode(pinRed, OUTPUT); 
  pinMode(pinYellow, OUTPUT); 
  pinMode(pinGreen, OUTPUT); 
  pinMode(pinBtn, INPUT_PULLUP); 
  for(int i = 0; i < 7; i++) pinMode(pinsDigit[i], OUTPUT);
  Serial.begin(9600);
  
  showNumber("42962");
  timestamp = millis();
}

void handleStates(){
  if(state == 0){
  	digitalWrite(pinRed, HIGH);
    digitalWrite(pinYellow, LOW);
    digitalWrite(pinGreen, LOW);
    
    int timeMs = (timeStop - (millis() - timestamp));
    int timeS = timeMs/1000.0 > 0 ? timeMs/1000.0 : 0;
    Serial.print("Time to prepare to go: ");
    Serial.print(timeS);
    Serial.println(" s");
    showDigit(timeS);
  }
  else if(state == 1){
  	digitalWrite(pinRed, HIGH);
    digitalWrite(pinYellow, HIGH);
    digitalWrite(pinGreen, LOW);
    showDigit(0);
  }
  else if(state == 2){
  	digitalWrite(pinRed, LOW);
    digitalWrite(pinYellow, LOW);
    digitalWrite(pinGreen, HIGH);
    showDigit(0);
  }
  else if(state == 3){
  	digitalWrite(pinRed, LOW);
    digitalWrite(pinYellow, HIGH);
    digitalWrite(pinGreen, LOW);
    showDigit(0);
  }
}

void handleStatesMovements(){
  if(state == 0 && millis() - timestamp > timeStop){
    state = 1;
  	timestamp = millis();
  }
  else if(state == 0 && digitalRead(pinBtn) == LOW){
    delay(20);
    if(digitalRead(pinBtn) == LOW){
      state = 1;
      timestamp = millis();
    }
  }
  else if(state == 1 && millis() - timestamp > timePrepareToGo){
    state = 2;
  	timestamp = millis();
  }
  else if(state == 2 && millis() - timestamp > timeGo){
    state = 3;
  	timestamp = millis();
  }
  else if(state == 3 && millis() - timestamp > timePrepareToStop){
    state = 0;
  	timestamp = millis();
  }
}

void off(int segment){
  	digitalWrite(segment, LOW);
}

void on(int segment){
  digitalWrite(segment, HIGH);
}

void showDigit(int digit){
  if(digit == 0){on(A);on(B);on(C);on(D);on(E);on(F);off(G);}
  if(digit == 1){off(A);on(B);on(C);off(D);off(E);off(F);off(G);}
  if(digit == 2){on(A);on(B);off(C);on(D);on(E);off(F);on(G);}
  if(digit == 3){on(A);on(B);on(C);on(D);off(E);off(F);on(G);}
  if(digit == 4){off(A);on(B);on(C);off(D);off(E);on(F);on(G);}
  if(digit == 5){on(A);off(B);on(C);on(D);off(E);on(F);on(G);}
  if(digit == 6){on(A);off(B);on(C);on(D);on(E);on(F);on(G);}
  if(digit == 7){on(A);on(B);on(C);off(D);off(E);off(F);off(G);}
  if(digit == 8){on(A);on(B);on(C);on(D);on(E);on(F);on(G);}
  if(digit == 9){on(A);on(B);on(C);on(D);off(E);on(F);on(G);}
}

void showNumber(String number){
  for(int i = 0; i < number.length(); i++){
    showDigit(number[i] - '0');
    delay(1000);
  }
}
  
void loop(){ 
  handleStates();
  handleStatesMovements();
  delay(100);
}