// TUGAS BESAR SISTEM MIKROKONTROLER - Smart Traffic Light dengan Sensor dan Sistem Prioritas

// Required Libraries
#include <LiquidCrystal_I2C.h> // Library untuk LCD I2C
// #include <Adafruit_GFX.h> // Jika menggunakan OLED
// #include <Adafruit_SSD1306.h> // Jika menggunakan OLED

// Define Pins (Sesuai dengan tata letak Arduino Mega 2560 yang disarankan)

// Sensor Ultrasonik (HC-SR04) - Pins ini akan tetap didefinisikan,
// namun untuk simulasi Wokwi, pembacaan jarak akan disederhanakan/digantikan oleh tombol.
#define TRIG_PIN_NORTH 22
#define ECHO_PIN_NORTH 23
#define TRIG_PIN_EAST 24
#define ECHO_PIN_EAST 25
#define TRIG_PIN_SOUTH 26
#define ECHO_PIN_SOUTH 27
#define TRIG_PIN_WEST 28
#define ECHO_PIN_WEST 29

// LED Lampu Lalu Lintas (4 set Merah, Kuning, Hijau)
// Utara
#define RED_LED_NORTH 30
#define YELLOW_LED_NORTH 31
#define GREEN_LED_NORTH 32
// Timur
#define RED_LED_EAST 33
#define YELLOW_LED_EAST 34
#define GREEN_LED_EAST 35
// Selatan
#define RED_LED_SOUTH 36
#define YELLOW_LED_SOUTH 37
#define GREEN_LED_SOUTH 38
// Barat
#define RED_LED_WEST 39
#define YELLOW_LED_WEST 40
#define GREEN_LED_WEST 41

// Buzzer
#define BUZZER_PIN 10

// Tombol Darurat (Simulasi kendaraan darurat dan deteksi antrian)
#define EMERGENCY_BUTTON_NORTH 2
#define EMERGENCY_BUTTON_EAST 3
#define EMERGENCY_BUTTON_SOUTH 4
#define EMERGENCY_BUTTON_WEST 5

// LCD/OLED
// Inisialisasi objek LCD I2C. Ganti 0x27 dengan alamat I2C LCD Anda jika berbeda.
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Variabel Global
int carCountNorth = 0; // Jumlah kendaraan di jalur Utara
int carCountEast = 0;  // Jumlah kendaraan di jalur Timur
int carCountSouth = 0; // Jumlah kendaraan di jalur Selatan
int carCountWest = 0;  // Jumlah kendaraan di jalur Barat

int greenLightDuration; // Durasi lampu hijau berdasarkan jumlah kendaraan
const int MIN_GREEN_DURATION = 10000; // Durasi lampu hijau minimum (10 detik)
const int MAX_GREEN_DURATION = 20000; // Durasi lampu hijau maksimum (20 detik)
const int EMERGENCY_GREEN_DURATION = 10000; // Durasi lampu hijau darurat (10 detik)
const int YELLOW_LIGHT_DURATION = 2000; // Durasi lampu kuning (2 detik)

enum TrafficDirection { NORTH, EAST, SOUTH, WEST };
TrafficDirection currentGreenDirection; // Arah yang sedang hijau

unsigned long previousMillisTrafficLight = 0; // Waktu terakhir perubahan lampu lalu lintas
unsigned long previousMillisEmergency = 0;    // Waktu terakhir mode darurat aktif
bool emergencyActive = false;                 // Status mode darurat
TrafficDirection emergencyDirection;           // Arah kendaraan darurat

// Variabel untuk debouncing tombol
unsigned long lastDebounceTime[4] = {0, 0, 0, 0};
unsigned long debounceDelay = 50; // milliseconds

// Variabel untuk efek kedip LCD pada mode darurat
bool blinkState = false;
unsigned long previousMillisBlink = 0;
const long BLINK_INTERVAL = 500; // Interval kedip (500 ms)

// --- Implementasi Fungsi ---
// Semua fungsi pembantu didefinisikan sebelum setup() dan loop()

// Menentukan arah dengan antrian terpanjang
TrafficDirection getLongestQueueDirection() {
  int maxCars = 0;
  TrafficDirection longestQueueDir = NORTH; // Default awal

  int carCounts[4] = {carCountNorth, carCountEast, carCountSouth, carCountWest};

  // Cari antrian terpanjang di antara semua jalur
  for (int i = 0; i < 4; i++) {
    if (carCounts[i] > maxCars) {
      maxCars = carCounts[i];
      longestQueueDir = (TrafficDirection)i;
    }
  }

  // Jika semua jalur kosong, putar siklus secara normal
  if (maxCars == 0) {
    return (TrafficDirection)((currentGreenDirection + 1) % 4);
  } else {
    return longestQueueDir;
  }
}

// Fungsi bantu untuk mematikan semua LED
void turnOffAllLights() {
  digitalWrite(RED_LED_NORTH, LOW);
  digitalWrite(YELLOW_LED_NORTH, LOW);
  digitalWrite(GREEN_LED_NORTH, LOW);
  digitalWrite(RED_LED_EAST, LOW);
  digitalWrite(YELLOW_LED_EAST, LOW);
  digitalWrite(GREEN_LED_EAST, LOW);
  digitalWrite(RED_LED_SOUTH, LOW);
  digitalWrite(YELLOW_LED_SOUTH, LOW);
  digitalWrite(GREEN_LED_SOUTH, LOW);
  digitalWrite(RED_LED_WEST, LOW);
  digitalWrite(YELLOW_LED_WEST, LOW);
  digitalWrite(GREEN_LED_WEST, LOW);
}

// Fungsi bantu untuk menyalakan lampu kuning arah tertentu
void setYellowLight(TrafficDirection dir) {
  switch (dir) {
    case NORTH:
      digitalWrite(GREEN_LED_NORTH, LOW);
      digitalWrite(YELLOW_LED_NORTH, HIGH);
      break;
    case EAST:
      digitalWrite(GREEN_LED_EAST, LOW);
      digitalWrite(YELLOW_LED_EAST, HIGH);
      break;
    case SOUTH:
      digitalWrite(GREEN_LED_SOUTH, LOW);
      digitalWrite(YELLOW_LED_SOUTH, HIGH);
      break;
    case WEST:
      digitalWrite(GREEN_LED_WEST, LOW);
      digitalWrite(YELLOW_LED_WEST, HIGH);
      break;
  }
}

// Fungsi bantu untuk menyalakan lampu merah arah tertentu
void setRedLight(TrafficDirection dir) {
  switch (dir) {
    case NORTH:
      digitalWrite(YELLOW_LED_NORTH, LOW);
      digitalWrite(RED_LED_NORTH, HIGH);
      break;
    case EAST:
      digitalWrite(YELLOW_LED_EAST, LOW);
      digitalWrite(RED_LED_EAST, HIGH);
      break;
    case SOUTH:
      digitalWrite(YELLOW_LED_SOUTH, LOW);
      digitalWrite(RED_LED_SOUTH, HIGH);
      break;
    case WEST:
      digitalWrite(YELLOW_LED_WEST, LOW);
      digitalWrite(RED_LED_WEST, HIGH);
      break;
  }
}

// Fungsi bantu untuk menyalakan lampu hijau arah tertentu
void setGreenLight(TrafficDirection dir) {
  switch (dir) {
    case NORTH:
      digitalWrite(RED_LED_NORTH, LOW);
      digitalWrite(GREEN_LED_NORTH, HIGH);
      break;
    case EAST:
      digitalWrite(RED_LED_EAST, LOW);
      digitalWrite(GREEN_LED_EAST, HIGH);
      break;
    case SOUTH:
      digitalWrite(RED_LED_SOUTH, LOW);
      digitalWrite(GREEN_LED_SOUTH, HIGH);
      break;
    case WEST:
      digitalWrite(RED_LED_WEST, LOW);
      digitalWrite(GREEN_LED_WEST, HIGH);
      break;
  }
}

// Mengatur lampu lalu lintas untuk arah yang diberikan sebagai hijau
// dan mengatur lampu merah untuk arah lainnya
void setTrafficLights(TrafficDirection greenDir) {
  // Matikan semua lampu terlebih dahulu
  turnOffAllLights(); // Memanggil fungsi bantu untuk mematikan semua

  // Set lampu sesuai arah yang hijau dan merah untuk yang lain
  switch (greenDir) {
    case NORTH:
      digitalWrite(GREEN_LED_NORTH, HIGH);
      digitalWrite(RED_LED_EAST, HIGH);
      digitalWrite(RED_LED_SOUTH, HIGH);
      digitalWrite(RED_LED_WEST, HIGH);
      break;
    case EAST:
      digitalWrite(GREEN_LED_EAST, HIGH);
      digitalWrite(RED_LED_NORTH, HIGH);
      digitalWrite(RED_LED_SOUTH, HIGH);
      digitalWrite(RED_LED_WEST, HIGH);
      break;
    case SOUTH:
      digitalWrite(GREEN_LED_SOUTH, HIGH);
      digitalWrite(RED_LED_NORTH, HIGH);
      digitalWrite(RED_LED_EAST, HIGH);
      digitalWrite(RED_LED_WEST, HIGH);
      break;
    case WEST:
      digitalWrite(GREEN_LED_WEST, HIGH);
      digitalWrite(RED_LED_NORTH, HIGH);
      digitalWrite(RED_LED_EAST, HIGH);
      digitalWrite(RED_LED_SOUTH, HIGH);
      break;
  }
}

// Memperbarui tampilan LCD/OLED dengan status lalu lintas saat ini
void updateDisplay(TrafficDirection activeDir, int countdown) {
  lcd.setCursor(0, 0);
  lcd.print("Aktif: ");
  switch (activeDir) {
    case NORTH: lcd.print("Utara "); break;
    case EAST: lcd.print("Timur "); break;
    case SOUTH: lcd.print("Selatan"); break;
    case WEST: lcd.print("Barat "); break;
  }
  lcd.print("      "); // Bersihkan sisa spasi

  lcd.setCursor(0, 1);
  lcd.print("Waktu: ");
  lcd.print(countdown);
  lcd.print("s   "); // Bersihkan sisa spasi

  // Tampilkan jumlah kendaraan di serial monitor
  Serial.print("Antrian Utara: "); Serial.print(carCountNorth);
  Serial.print(", Timur: "); Serial.print(carCountEast);
  Serial.print(", Selatan: "); Serial.print(carCountSouth);
  Serial.print(", Barat: "); Serial.println(carCountWest);
}

// Menangani deteksi kendaraan darurat dan memprioritaskan lalu lintas
void handleEmergency(TrafficDirection emergencyDir) {
  // Hanya aktifkan jika belum dalam mode darurat.
  // Jika sudah dalam mode darurat, permintaan darurat baru akan diabaikan
  // untuk memastikan satu mode darurat berjalan sampai selesai.
  if (!emergencyActive) {
    emergencyActive = true;
    emergencyDirection = emergencyDir;
    digitalWrite(BUZZER_PIN, HIGH); // Aktifkan buzzer untuk alarm
    setTrafficLights(emergencyDir); // Nyalakan lampu hijau untuk arah darurat dan merah untuk yang lain
    previousMillisEmergency = millis(); // Mulai timer darurat
    Serial.print("!!! DARURAT! Arah "); Serial.print(emergencyDir); Serial.println(" Hijau !!!");
    lcd.clear(); // Bersihkan layar untuk menampilkan pesan darurat awal
    // Reset status kedip untuk awal mode darurat
    blinkState = true;
    previousMillisBlink = millis();
  } else {
    // Jika mode darurat sudah aktif, dan ada permintaan darurat dari arah yang berbeda
    // maka kita bisa memberi tahu di serial monitor bahwa permintaan baru diabaikan
    if (emergencyDirection != emergencyDir) {
      Serial.println("Permintaan darurat lain diabaikan, mode darurat sudah aktif.");
    }
  }
}

void setup() {
  Serial.begin(9600); // Inisialisasi Serial Monitor untuk debugging

  // Inisialisasi pin LED sebagai OUTPUT
  pinMode(RED_LED_NORTH, OUTPUT);
  pinMode(YELLOW_LED_NORTH, OUTPUT);
  pinMode(GREEN_LED_NORTH, OUTPUT);
  pinMode(RED_LED_EAST, OUTPUT);
  pinMode(YELLOW_LED_EAST, OUTPUT);
  pinMode(GREEN_LED_EAST, OUTPUT);
  pinMode(RED_LED_SOUTH, OUTPUT);
  pinMode(YELLOW_LED_SOUTH, OUTPUT);
  pinMode(GREEN_LED_SOUTH, OUTPUT);
  pinMode(RED_LED_WEST, OUTPUT);
  pinMode(YELLOW_LED_WEST, OUTPUT);
  pinMode(GREEN_LED_WEST, OUTPUT);

  // Inisialisasi pin Buzzer sebagai OUTPUT
  pinMode(BUZZER_PIN, OUTPUT);

  // Inisialisasi pin Sensor Ultrasonik (Trig sebagai OUTPUT, Echo sebagai INPUT)
  pinMode(TRIG_PIN_NORTH, OUTPUT);
  pinMode(ECHO_PIN_NORTH, INPUT);
  pinMode(TRIG_PIN_EAST, OUTPUT);
  pinMode(ECHO_PIN_EAST, INPUT);
  pinMode(TRIG_PIN_SOUTH, OUTPUT);
  pinMode(ECHO_PIN_SOUTH, INPUT);
  pinMode(TRIG_PIN_WEST, OUTPUT);
  pinMode(ECHO_PIN_WEST, INPUT);

  // Inisialisasi pin Tombol Darurat sebagai INPUT_PULLUP
  pinMode(EMERGENCY_BUTTON_NORTH, INPUT_PULLUP);
  pinMode(EMERGENCY_BUTTON_EAST, INPUT_PULLUP);
  pinMode(EMERGENCY_BUTTON_SOUTH, INPUT_PULLUP);
  pinMode(EMERGENCY_BUTTON_WEST, INPUT_PULLUP);

  // Inisialisasi LCD/OLED
  lcd.init();      // Inisialisasi LCD
  lcd.backlight(); // Nyalakan backlight LCD
  lcd.print("Smart Traffic Light"); // Tampilkan pesan awal
  lcd.setCursor(0,1);
  lcd.print("Starting...");
  delay(2000);
  lcd.clear();

  // Status awal: Semua lampu merah, kemudian mulai dengan Utara hijau
  currentGreenDirection = NORTH; // Mulai dengan arah Utara hijau
  setTrafficLights(currentGreenDirection); // Set lampu hijau Utara dan merah untuk yang lain
  previousMillisTrafficLight = millis(); // Atur waktu awal siklus
  Serial.println("System Started. North Green.");
}

void loop() {
  unsigned long currentMillis = millis();

  // --- Pembacaan Tombol dan Simulasi Antrian/Darurat ---
  if (digitalRead(EMERGENCY_BUTTON_NORTH) == LOW) {
    if (currentMillis - lastDebounceTime[NORTH] > debounceDelay) {
      lastDebounceTime[NORTH] = currentMillis;
      if (!emergencyActive) {
          carCountNorth = min(carCountNorth + 1, 10);
          Serial.print("North Car Count: "); Serial.println(carCountNorth);
      }
      handleEmergency(NORTH);
    }
  }
  if (digitalRead(EMERGENCY_BUTTON_EAST) == LOW) {
    if (currentMillis - lastDebounceTime[EAST] > debounceDelay) {
      lastDebounceTime[EAST] = currentMillis;
      if (!emergencyActive) {
          carCountEast = min(carCountEast + 1, 10);
          Serial.print("East Car Count: "); Serial.println(carCountEast);
      }
      handleEmergency(EAST);
    }
  }
  if (digitalRead(EMERGENCY_BUTTON_SOUTH) == LOW) {
    if (currentMillis - lastDebounceTime[SOUTH] > debounceDelay) {
      lastDebounceTime[SOUTH] = currentMillis;
      if (!emergencyActive) {
          carCountSouth = min(carCountSouth + 1, 10);
          Serial.print("South Car Count: "); Serial.println(carCountSouth);
      }
      handleEmergency(SOUTH);
    }
  }
  if (digitalRead(EMERGENCY_BUTTON_WEST) == LOW) {
    if (currentMillis - lastDebounceTime[WEST] > debounceDelay) {
      lastDebounceTime[WEST] = currentMillis;
      if (!emergencyActive) {
          carCountWest = min(carCountWest + 1, 10);
          Serial.print("West Car Count: "); Serial.println(carCountWest);
      }
      handleEmergency(WEST);
    }
  }

  // --- Logika Lampu Lalu Lintas Normal ---
  if (!emergencyActive) {
    int maxCarCount = max(max(carCountNorth, carCountEast), max(carCountSouth, carCountWest));
    greenLightDuration = MIN_GREEN_DURATION + (maxCarCount * 1000);
    if (greenLightDuration > MAX_GREEN_DURATION) greenLightDuration = MAX_GREEN_DURATION;

    int trafficLightStateDuration = greenLightDuration;

    if (currentMillis - previousMillisTrafficLight >= trafficLightStateDuration) {
      previousMillisTrafficLight = currentMillis;

      setYellowLight(currentGreenDirection);
      Serial.print("Direction "); Serial.print(currentGreenDirection); Serial.println(" Yellow.");
      delay(YELLOW_LIGHT_DURATION);

      setRedLight(currentGreenDirection);
      Serial.print("Direction "); Serial.print(currentGreenDirection); Serial.println(" Red.");

      switch (currentGreenDirection) {
        case NORTH: carCountNorth = 0; break;
        case EAST: carCountEast = 0; break;
        case SOUTH: carCountSouth = 0; break;
        case WEST: carCountWest = 0; break;
      }
      Serial.print("Car Count Reset for "); Serial.print(currentGreenDirection); Serial.println(".");

      TrafficDirection nextGreenDir = getLongestQueueDirection();

      if (carCountNorth == 0 && carCountEast == 0 && carCountSouth == 0 && carCountWest == 0) {
        currentGreenDirection = (TrafficDirection)((currentGreenDirection + 1) % 4);
      } else {
        currentGreenDirection = nextGreenDir;
      }

      setTrafficLights(currentGreenDirection);
      Serial.print("New Green Direction: "); Serial.println(currentGreenDirection);
    }

    int countdown = (trafficLightStateDuration - (currentMillis - previousMillisTrafficLight)) / 1000;
    if (countdown < 0) countdown = 0;

    updateDisplay(currentGreenDirection, countdown);

  } else { // Mode Kendaraan Darurat Aktif
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillisEmergency >= EMERGENCY_GREEN_DURATION) {
      emergencyActive = false;
      digitalWrite(BUZZER_PIN, LOW);
      setRedLight(emergencyDirection); // Pastikan lampu darurat kembali merah
      Serial.println("Emergency Mode Ended. Resuming normal cycle.");
      previousMillisTrafficLight = millis();
      lcd.clear();
      lcd.print("Traffic Normal");
      blinkState = false;
      previousMillisBlink = 0;
    } else {
      if (currentMillis - previousMillisBlink >= BLINK_INTERVAL) {
        previousMillisBlink = currentMillis;
        blinkState = !blinkState;
        lcd.setCursor(0, 0);

        if (blinkState) {
          lcd.print("!!! DARURAT !!!");
        } else {
          lcd.print("               ");
        }
      }

      lcd.setCursor(0, 1);
      lcd.print("Arah: ");
      switch(emergencyDirection) {
        case NORTH: lcd.print("Utara"); break;
        case EAST: lcd.print("Timur"); break;
        case SOUTH: lcd.print("Selatan"); break;
        case WEST: lcd.print("Barat"); break;
      }
      int emergencyCountdown = (EMERGENCY_GREEN_DURATION - (currentMillis - previousMillisEmergency)) / 1000;
      lcd.print(" ");
      lcd.print(emergencyCountdown);
      lcd.print("s");
    }
  }
}