#include <Keypad.h>
#include <Servo.h>
Servo sv1,sv2,sv3,sv4;

#include <Wire.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

const byte KEYPAD_ROWS = 4;
const byte KEYPAD_COLS = 4;
byte rowPins[KEYPAD_ROWS] = {5, 4, A5, A4};
byte colPins[KEYPAD_COLS] = {A3, A2, A1, A0};
char keys[KEYPAD_ROWS][KEYPAD_COLS] = {
  {'1', '2', '3', 'A'}, {'4', '5', '6', 'B'}, {'7', '8', '9', 'C'}, {'*', '0', '#', 'D'} };
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, KEYPAD_ROWS, KEYPAD_COLS);

//variabel lampu
int lam11 = 6,lam12 = 7, lam13 = 8;
int lam21 = 9,lam22 = 10, lam23 = 11;
int lam31 = 17, lam32 = 18, lam33 = 19;
int lam41 = 14, lam42 = 15, lam43 = 16;

void setup() {
  Serial.begin(9600);
  pinMode(lam11, OUTPUT); pinMode(lam21, OUTPUT); pinMode(lam31, OUTPUT);
  pinMode(lam12, OUTPUT); pinMode(lam22, OUTPUT); pinMode(lam32, OUTPUT);
  pinMode(lam13, OUTPUT); pinMode(lam23, OUTPUT); pinMode(lam33, OUTPUT);

  sv1.attach(12);
  sv2.attach(13);
  sv3.attach(2);
  sv4.attach(3);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
}

void loop() {
  unsigned long waktu,waktukini = 0,waktuakhir = 0;
  int i =1;

  
  // check if "interval" time has passed (1000 milliseconds)
  // if ((unsigned long)(currentMillis - previousMillis) >= interval) {

  char tombol = keypad.getKey();
  if (tombol!= NO_KEY){
    if(tombol == '0'){

      display.clearDisplay();
      display.setTextSize(2);
      display.setTextColor(WHITE);
      display.setCursor(30,10);
      display.println("STOP!!!");
      Serial.println("STOP!!!");
      display.display();

      digitalWrite(lam13, LOW);
      digitalWrite(lam23, LOW);
      digitalWrite(lam33, LOW);
      digitalWrite(lam43, LOW);

      digitalWrite(lam11, HIGH);
      digitalWrite(lam21, HIGH);
      digitalWrite(lam31, HIGH);
      digitalWrite(lam41, HIGH);

      sv1.write(90);
      sv2.write(90);
      sv3.write(90);
      sv4.write(90);
    }
    if (tombol == '1'){
      display.clearDisplay();
      display.setTextSize(2);
      display.setTextColor(WHITE);
      display.setCursor(30,10);
      display.println("HIJAU");
      Serial.println("HIJAU");
      display.display();

      digitalWrite(lam11, LOW);
      digitalWrite(lam21, LOW);
      digitalWrite(lam31, LOW);
      digitalWrite(lam41, LOW);

      digitalWrite(lam13, HIGH);
      digitalWrite(lam23, HIGH);
      digitalWrite(lam33, HIGH);
      digitalWrite(lam43, HIGH);

      sv1.write(0);
      sv2.write(0);
      sv3.write(0);
      sv4.write(0);
    }
    // if (tombol == '2'){

      

      while (tombol == '2'){
        waktukini=millis();
        waktu = waktukini - waktuakhir;
        if (waktu>=3500){
          display.clearDisplay();
          display.setTextSize(2);
          display.setTextColor(WHITE);
          display.setCursor(30,10);
          display.println("NORMAL");
          Serial.println("NORMAL");
          display.display();

          digitalWrite(lam11, LOW); digitalWrite(lam12, LOW); digitalWrite(lam13, LOW);
          digitalWrite(lam21, LOW); digitalWrite(lam22, LOW); digitalWrite(lam23, LOW);
          digitalWrite(lam31, LOW); digitalWrite(lam32, LOW); digitalWrite(lam33, LOW);
          digitalWrite(lam41, LOW); digitalWrite(lam42, LOW); digitalWrite(lam43, LOW);

          digitalWrite(lam11, LOW);
          digitalWrite(lam21, HIGH);
          digitalWrite(lam31, HIGH);
          digitalWrite(lam41, HIGH);
          digitalWrite(lam13, HIGH);
          sv1.write(90);
          sv2.write(0);
          sv3.write(90);
          sv4.write(90);
          delay(1000);
          digitalWrite(lam13, LOW);
          digitalWrite(lam12, HIGH);
          delay(100);

          digitalWrite(lam12, LOW);
          digitalWrite(lam11, HIGH);
          digitalWrite(lam21, LOW);
          digitalWrite(lam31, HIGH);
          digitalWrite(lam41, HIGH);
          digitalWrite(lam23, HIGH);
          sv1.write(90);
          sv2.write(90);
          sv3.write(0);
          sv4.write(90);
          delay(1000);
          digitalWrite(lam23, LOW);
          digitalWrite(lam22, HIGH);
          delay(100);
          
          digitalWrite(lam22, LOW);
          digitalWrite(lam11, HIGH);
          digitalWrite(lam21, HIGH);
          digitalWrite(lam31, LOW);
          digitalWrite(lam41, HIGH);
          digitalWrite(lam33, HIGH);
          sv1.write(90);
          sv2.write(90);
          sv3.write(90);
          sv4.write(0);
          delay(1000);
          digitalWrite(lam33, LOW);
          digitalWrite(lam32, HIGH);
          delay(100);
          
          digitalWrite(lam32, LOW);
          digitalWrite(lam11, HIGH);
          digitalWrite(lam21, HIGH);
          digitalWrite(lam31, HIGH);
          digitalWrite(lam41, LOW);
          digitalWrite(lam43, HIGH);
          sv1.write(0);
          sv2.write(90);
          sv3.write(90);
          sv4.write(90);
          delay(1000);
          digitalWrite(lam43, LOW);
          digitalWrite(lam42, HIGH);
          delay(100);
          i++;
          if(i==2)i=1;
          waktuakhir=waktukini;
        }
        char tombol = keypad.getKey();
        if (tombol!= NO_KEY){
         if(tombol != '2'){
           Serial.println("Break");
            break;
          }

        }
      }
  }
}