//KELOMPOK 5 IOT
//Muhammad Ridwan - 2001010016
//Kurnia Sandi - 190120201047
//Nia Afrianti - 2001010024
#include <Keypad.h>
#include<Servo.h>
#include <Wire.h>
#include<SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define address 0x3C

//untuk OLED
byte lebar = 128, panjang = 64;
int reset = -1;
Adafruit_SSD1306 OLED(lebar, panjang, &Wire, -1);

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

//untuk servo
Servo sv1, sv2, sv3, sv4;

//untuk led simpang 1
int Merah_1 = A0, Kuning_1 = A1, Hijau_1 = A2;
//untuk led simpang 2
int Merah_2 = A3, Kuning_2 = A4, Hijau_2 = A5;
//untuk led simpang 3
int Merah_3 = A6, Kuning_3 = A7, Hijau_3 = A8;
//untuk led simpang 4
int Merah_4 = A9, Kuning_4 = A10, Hijau_4 = A11;
unsigned long jedaWaktu = 0, waktuSekarang = 0, waktuAkhir = 0;
int i = 1;
void setup() {
  Serial.begin(9600);
  sv1.attach(10);
  sv2.attach(44);
  sv3.attach(45);
  sv4.attach(46);
  pinMode(Merah_1, OUTPUT); pinMode(Kuning_1, OUTPUT); pinMode(Hijau_1, OUTPUT);
  pinMode(Merah_2, OUTPUT); pinMode(Kuning_2, OUTPUT); pinMode(Hijau_2, OUTPUT);
  pinMode(Merah_3, OUTPUT); pinMode(Kuning_3, OUTPUT); pinMode(Hijau_3, OUTPUT);
  pinMode(Merah_4, OUTPUT); pinMode(Kuning_4, OUTPUT); pinMode(Hijau_4, OUTPUT);
  OLED.begin(SSD1306_SWITCHCAPVCC, address);
  OLED.clearDisplay();

}
void loop() {
  char HasilTekanKeypad = keypad.getKey();
  if (HasilTekanKeypad != NO_KEY) {
    OLED.clearDisplay();
    if (HasilTekanKeypad == '1') { //kondisi semua lampu merah + gerbang tertutup
      String keadaan = "SEMUA MERAH";
      kondisiLED_merah_hijau(keadaan);
      Serial.println("Maintenance Mode\nSemua Lampu Merah ON\nSemua Gerbang Ditutup\n");
      OLED.setTextSize(1); OLED.setTextColor(WHITE);
      OLED.setCursor(16, 0); OLED.println("Maintenance Mode");
      OLED.setCursor(2, 16); OLED.println("Semua Lampu Merah ON");
      OLED.setCursor(1, 32); OLED.println("Semua Gerbang Ditutup");
      OLED.display();
    }
    else if (HasilTekanKeypad == '0') { //kondisi semua lampu hijau + gerbang terbuka
      String keadaan = "SEMUA HIJAU";
      kondisiLED_merah_hijau(keadaan);
      Serial.println("Chaos Mode\nSemua Lampu Hijau ON\nSemua Gerbang Terbuka\n");
      OLED.setTextSize(1); OLED.setTextColor(WHITE);
      OLED.setCursor(32, 0); OLED.println("Chaos Mode");
      OLED.setCursor(2, 16); OLED.println("Semua Lampu Hijau ON");
      OLED.setCursor(1, 32); OLED.println("Semua Gerbang Terbuka");
      OLED.display();
    }

    while (HasilTekanKeypad == '2') { //kondisi normal
      waktuSekarang = millis();
      jedaWaktu = waktuSekarang - waktuAkhir;
      if (jedaWaktu >= 3000) {
        Serial.print(jedaWaktu / 1000); Serial.println(" detik");
        Serial.print("Simpang : "); Serial.println(i);

        kondisiNormal(i, 1500); //simpang,delayKuning
        i++;
        if (i == 5)i = 1;
        waktuAkhir = waktuSekarang;
      }

      char keyOut = keypad.getKey();
      if (keyOut != NO_KEY) {
        if (keyOut != '2') {
          String keadaan = "SEMUA MERAH";
          kondisiLED_merah_hijau(keadaan);
          Serial.println("STOPPED");
          OLED.clearDisplay();
          OLED.setTextSize(1); OLED.setTextColor(WHITE);
          OLED.setCursor(28, 0); OLED.println("2. Normal Mode");
          OLED.setCursor(28, 16); OLED.println("1. Semua Merah");
          OLED.setCursor(28, 32); OLED.println("0. Semua Hijau");
          OLED.display();
          i = 1;
          break;
        }
      }
    }
  }
}

void kondisiLED_merah_hijau(String a) {
  int logik_merah;
  int logik_hijau;
  int sudut;
  if (a == "SEMUA MERAH") {
    logik_merah = 1;
    logik_hijau = !logik_merah;
    //semua gerbang/servo tertutup
    sudut = 90;
  }
  else if (a == "SEMUA HIJAU") {
    logik_hijau = 1;
    logik_merah = !logik_hijau;
    //semua gerbang/servo terbuka
    sudut = 0;
  }
  //kuning pasti off
  digitalWrite(Kuning_1, 0);
  digitalWrite(Kuning_2, 0);
  digitalWrite(Kuning_3, 0);
  digitalWrite(Kuning_4, 0);
  //merah
  digitalWrite(Merah_1, logik_merah);
  digitalWrite(Merah_2, logik_merah);
  digitalWrite(Merah_3, logik_merah);
  digitalWrite(Merah_4, logik_merah);
  //hjau
  digitalWrite(Hijau_1, logik_hijau);
  digitalWrite(Hijau_2, logik_hijau);
  digitalWrite(Hijau_3, logik_hijau);
  digitalWrite(Hijau_4, logik_hijau);
  //servo simpang 1-4
  sv1.write(sudut);
  sv2.write(sudut);
  sv3.write(sudut);
  sv4.write(sudut);
}


void kondisiNormal(int simpang, int lama_warna_kuning) {
  semua_LED_mati_dulu();
  if (simpang == 1) { //simpang 1
    digitalWrite(Merah_2, HIGH); digitalWrite(Kuning_2, LOW); digitalWrite(Kuning_2, LOW);
    digitalWrite(Merah_3, HIGH); digitalWrite(Kuning_3, LOW); digitalWrite(Kuning_3, LOW);
    digitalWrite(Merah_4, HIGH); digitalWrite(Kuning_4, LOW); digitalWrite(Kuning_4, LOW);
    digitalWrite(Merah_1, LOW); digitalWrite(Kuning_1, HIGH); digitalWrite(Hijau_1, LOW);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 1");
    OLED.setCursor(20, 32); OLED.println("Gerbang Tertutup");
    OLED.display();
    sv4.write(90);
    delay(lama_warna_kuning);
    digitalWrite(Merah_1, LOW); digitalWrite(Kuning_1, LOW); digitalWrite(Hijau_1, HIGH);
    sv1.write(0); sv2.write(90); sv3.write(90);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 1");
    OLED.setCursor(20, 32); OLED.println("Gerbang Terbuka");
    OLED.display();

  }
  else if (simpang == 2) { //simpang 2
    digitalWrite(Merah_1, HIGH); digitalWrite(Kuning_1, LOW); digitalWrite(Kuning_1, LOW);
    digitalWrite(Merah_3, HIGH); digitalWrite(Kuning_3, LOW); digitalWrite(Kuning_3, LOW);
    digitalWrite(Merah_4, HIGH); digitalWrite(Kuning_4, LOW); digitalWrite(Kuning_4, LOW);
    digitalWrite(Merah_2, LOW); digitalWrite(Kuning_2, HIGH); digitalWrite(Hijau_2, LOW);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 2");
    OLED.setCursor(20, 32); OLED.println("Gerbang Tertutup");
    OLED.display();
    sv1.write(90);
    delay(lama_warna_kuning);
    digitalWrite(Merah_2, LOW); digitalWrite(Kuning_2, LOW); digitalWrite(Hijau_2, HIGH);
    sv2.write(0); sv3.write(90); sv4.write(90);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 2");
    OLED.setCursor(20, 32); OLED.println("Gerbang Terbuka");
    OLED.display();
  }
  else if (simpang == 3) { //simpang 3
    digitalWrite(Merah_2, HIGH); digitalWrite(Kuning_2, LOW); digitalWrite(Kuning_2, LOW);
    digitalWrite(Merah_1, HIGH); digitalWrite(Kuning_1, LOW); digitalWrite(Kuning_1, LOW);
    digitalWrite(Merah_4, HIGH); digitalWrite(Kuning_4, LOW); digitalWrite(Kuning_4, LOW);
    digitalWrite(Merah_3, LOW); digitalWrite(Kuning_3, HIGH); digitalWrite(Hijau_3, LOW);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 3");
    OLED.setCursor(20, 32); OLED.println("Gerbang Tertutup");
    OLED.display();
    sv2.write(90);
    delay(lama_warna_kuning);
    digitalWrite(Merah_3, LOW); digitalWrite(Kuning_3, LOW); digitalWrite(Hijau_3, HIGH);
    sv1.write(90); sv3.write(0); sv4.write(90);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 3");
    OLED.setCursor(20, 32); OLED.println("Gerbang Terbuka");
    OLED.display();
  }
  else if (simpang == 4) { //simpang 4
    digitalWrite(Merah_2, HIGH); digitalWrite(Kuning_2, LOW); digitalWrite(Kuning_2, LOW);
    digitalWrite(Merah_3, HIGH); digitalWrite(Kuning_3, LOW); digitalWrite(Kuning_3, LOW);
    digitalWrite(Merah_1, HIGH); digitalWrite(Kuning_1, LOW); digitalWrite(Kuning_1, LOW);
    digitalWrite(Merah_4, LOW); digitalWrite(Kuning_4, HIGH); digitalWrite(Hijau_4, LOW);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 4");
    OLED.setCursor(20, 32); OLED.println("Gerbang Tertutup");
    OLED.display();
    sv3.write(90);
    delay(lama_warna_kuning);
    digitalWrite(Merah_4, LOW); digitalWrite(Kuning_4, LOW); digitalWrite(Hijau_4, HIGH);
    sv1.write(90); sv2.write(90); sv4.write(0);
    OLED.clearDisplay();
    OLED.setTextSize(1); OLED.setTextColor(WHITE);
    OLED.setCursor(31, 0); OLED.println("Normal Mode");
    OLED.setCursor(31, 16); OLED.println("Simpang : 4");
    OLED.setCursor(20, 32); OLED.println("Gerbang Terbuka");
    OLED.display();
  }


}

void semua_LED_mati_dulu() {
  sv1.write(90); sv2.write(90); sv3.write(90); sv4.write(90);
  //kuning
  digitalWrite(Kuning_1, 0); digitalWrite(Kuning_2, 0); digitalWrite(Kuning_3, 0); digitalWrite(Kuning_4, 0);
  //merah
  digitalWrite(Merah_1, 0); digitalWrite(Merah_2, 0); digitalWrite(Merah_3, 0); digitalWrite(Merah_4, 0);
  //hjau
  digitalWrite(Hijau_1, 0); digitalWrite(Hijau_2, 0); digitalWrite(Hijau_3, 0); digitalWrite(Hijau_4, 0);
}