#include <Wire.h> 
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27,16,2);

int code[5];
int buttonState[6];
int codePosition = 0;

void setup() {
  pinMode(3, INPUT_PULLUP);
  pinMode(4, INPUT_PULLUP);
  pinMode(5, INPUT_PULLUP);
  pinMode(6, INPUT_PULLUP);
  pinMode(7, INPUT_PULLUP);
  pinMode(8, INPUT_PULLUP);
  pinMode(9, INPUT_PULLUP);
  pinMode(13, OUTPUT);  //yellow
  pinMode(12, OUTPUT);  //green
  pinMode(11, OUTPUT); //red
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("00000");
}

void loop() {
for (int i=0; i<6; i++) {
  buttonState[i] = digitalRead(i+3);
  if (buttonState[i] == 0) {
    code[codePosition] = i;
    codePosition += 1;
    delay(500);
    if (codePosition > 4) {
      codePosition = 0;
    }
  }
}
  if (digitalRead(9) == 0){
    codePosition = 0;
    code[0] = 0;
    code[1] = 0;
    code[2] = 0;
    code[3] = 0;
    code[4] = 0;
    code[5] = 0;
    digitalWrite(13, HIGH);
    delay(500);
    digitalWrite(13, LOW);
  }
  lcd.setCursor(0,0);
  lcd.print(code[0]);
  lcd.print(code[1]);
  lcd.print(code[2]);
  lcd.print(code[3]);
  lcd.print(code[4]);

  Serial.print(code[0]);
  Serial.print(code[1]);
  Serial.print(code[2]);
  Serial.print(code[3]);
  Serial.println(code[4]);
  if (code[0] == 1 && code[1] == 2 && code[2] == 5 && code[3] == 3 && code[4] == 5) {
    digitalWrite(12, HIGH);
    digitalWrite(11,LOW);
  } else { 
    digitalWrite(11, HIGH);
    digitalWrite(12,LOW);
  }  
}