#define SW1 4
#define SW2 16
#define A 17
#define B 5
#define C 18
#define D 19
#define E 21
#define F 23 
#define G 22

int num =0;
int last_time = 0;
bool check=true;
int count = 0;

void setup() 
{
  pinMode(SW1, INPUT_PULLUP); //ต่อแบบ pulldown 0 ปิด 1 เปิด ต่อ 2 สาย กลาง ขาที่จะต่อ กับ ขวาต่อกาว
  pinMode(SW2, INPUT_PULLUP);
  pinMode(A, OUTPUT);
  pinMode(B, OUTPUT);
  pinMode(C, OUTPUT);
  pinMode(D, OUTPUT);
  pinMode(E, OUTPUT);
  pinMode(F, OUTPUT);
  pinMode(G, OUTPUT);
}

void logic(){
  if((digitalRead(SW1)== LOW) && (digitalRead(SW2)==LOW))
  {
    zero();    
  }

  else if((digitalRead(SW1)==LOW) && (digitalRead(SW2)==HIGH) )
  {
    one();
  }

  else if((digitalRead(SW1)==HIGH) && (digitalRead(SW2)==LOW))
  {
    two();
  }
  
  else if((digitalRead(SW1)==HIGH) && (digitalRead(SW2)==HIGH))
  {
    three();
  }
}

void loop() 
{
  if(millis() - last_time >= 1000 && check==true){
    if(num < 3*2){
      if(num % 2 == 0){
        logic();
      }else{
        noblink();
      }
    }
    else{
      count++;
      noblink();
      if(count > 2){
        num=0;
        count=0;
        check=!check;
      }
    }
    num++;
    last_time = millis();
  }
  else if (millis() - last_time >= 500 && check==false){
    if(num <= 6*2){
      if(num % 2 == 0){
        logic();
      }else{
        noblink();
      }
    }
    else{
      count++;
      noblink();
      if(count > 2){
        num=0;
        count=0;
        check=!check;
      }
    }
    num++;
    last_time = millis();
  }
}

void noblink(){
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);

}

void zero() {
  digitalWrite(A, HIGH);
  digitalWrite(B, HIGH);
  digitalWrite(C, HIGH);
  digitalWrite(D, HIGH);
  digitalWrite(E, HIGH);
  digitalWrite(F, HIGH);
  digitalWrite(G, LOW);
}

void one(){
  digitalWrite(A, LOW);
  digitalWrite(B, HIGH);
  digitalWrite(C, HIGH);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}
void two(){
  digitalWrite(A, HIGH);
  digitalWrite(B, HIGH);
  digitalWrite(C, LOW);
  digitalWrite(D, HIGH);
  digitalWrite(E, HIGH);
  digitalWrite(F, LOW);
  digitalWrite(G, HIGH);
}
void three(){
  digitalWrite(A, HIGH);
  digitalWrite(B, HIGH);
  digitalWrite(C, HIGH);
  digitalWrite(D, HIGH);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, HIGH);
}