#include <Arduino_FreeRTOS.h> // library FreeRTOS untuk Arduino
#include <queue.h> // library Queue dari FreeRTOS

// Definisikan nomor pin untuk lampu lalu lintas
const int green1Pin = 0;   // Pin untuk lampu hijau arah 1
const int yellow1Pin = 1; 
const int red1Pin = 2;     

const int green2Pin = 10;  // Pin untuk lampu hijau arah 2
const int yellow2Pin = 9;  
const int red2Pin = 8;     

const int green3Pin = 4;   // Pin untuk lampu hijau arah 3
const int yellow3Pin = 5;  
const int red3Pin = 6;    

const int green4Pin = 13;  // Pin untuk lampu hijau arah 4
const int yellow4Pin = 12; 
const int red4Pin = 11;    

// Definisikan struktur untuk pesan queue
struct TrafficLightMessage {
  int greenPin;   // Pin lampu hijau
  int yellowPin;  
  int redPin;     
  int delayGreen; // Durasi delay untuk lampu hijau
  int delayYellow;
};

// Deklarasi queue handle
QueueHandle_t trafficLightQueue;

void setup() {
  // Atur pin LED sebagai output
  pinMode(green1Pin, OUTPUT);
  pinMode(yellow1Pin, OUTPUT);
  pinMode(red1Pin, OUTPUT);

  pinMode(green2Pin, OUTPUT);
  pinMode(yellow2Pin, OUTPUT);
  pinMode(red2Pin, OUTPUT);

  pinMode(green3Pin, OUTPUT);
  pinMode(yellow3Pin, OUTPUT);
  pinMode(red3Pin, OUTPUT);

  pinMode(green4Pin, OUTPUT);
  pinMode(yellow4Pin, OUTPUT);
  pinMode(red4Pin, OUTPUT);

  // Setting semua lampu menjadi merah awalnya 
  digitalWrite(red1Pin, HIGH);
  digitalWrite(red2Pin, HIGH);
  digitalWrite(red3Pin, HIGH);
  digitalWrite(red4Pin, HIGH);

  // Buat queue dengan kapasitas 10 pesan dalam antrian
  trafficLightQueue = xQueueCreate(10, sizeof(TrafficLightMessage));

  // Buat task untuk mengatur lampu lalu lintas
  xTaskCreate(controlTrafficLights, "Control Traffic Lights", 128, NULL, 1, NULL);

  // Kirim pesan awal ke queue
  TrafficLightMessage message; 
  
  // Mengatur pesan untuk arah 1
  message = {green1Pin, yellow1Pin, red1Pin, 5000, 2000};
  xQueueSend(trafficLightQueue, &message, portMAX_DELAY);
  
  // Mengatur pesan untuk arah 2
  message = {green2Pin, yellow2Pin, red2Pin, 5000, 2000};
  xQueueSend(trafficLightQueue, &message, portMAX_DELAY);
  
  // Mengatur pesan untuk arah 3
  message = {green3Pin, yellow3Pin, red3Pin, 5000, 2000};
  xQueueSend(trafficLightQueue, &message, portMAX_DELAY);
  
  // Mengatur pesan untuk arah 4
  message = {green4Pin, yellow4Pin, red4Pin, 5000, 2000};
  xQueueSend(trafficLightQueue, &message, portMAX_DELAY);
}

void loop() {
  // Tidak ada kode di loop karena semua diatur oleh FreeRTOS
}

// Task untuk mengatur lampu lalu lintas
void controlTrafficLights(void *pvParameters) {
  TrafficLightMessage message; // Variabel untuk menyimpan pesan dari queue
  while (1) { // loop berjalan terus
    // Menerima pesan dari queue
    if (xQueueReceive(trafficLightQueue, &message, portMAX_DELAY) == pdPASS) { // menerima pesan dari antrian 
      // Matikan semua lampu merah
      digitalWrite(red1Pin, HIGH); // matikan semua lampu
      digitalWrite(red2Pin, HIGH);
      digitalWrite(red3Pin, HIGH);
      digitalWrite(red4Pin, HIGH);

      // Nyalakan lampu hijau
      digitalWrite(message.redPin, LOW); // Matikan lampu merah untuk arah yang sesuai
      digitalWrite(message.greenPin, HIGH); // Nyalakan lampu hijau
      vTaskDelay(message.delayGreen / portTICK_PERIOD_MS); // Tunggu selama delay hijau
      digitalWrite(message.greenPin, LOW); // Matikan lampu hijau

      // Nyalakan lampu kuning
      digitalWrite(message.yellowPin, HIGH); // Nyalakan lampu kuning
      vTaskDelay(message.delayYellow / portTICK_PERIOD_MS); // Tunggu selama delay kuning
      digitalWrite(message.yellowPin, LOW); // Matikan lampu kuning

      // Kembali ke lampu merah
      digitalWrite(message.redPin, HIGH); // Nyalakan kembali lampu merah

      // Kirim pesan kembali ke queue untuk melanjutkan siklus
      xQueueSend(trafficLightQueue, &message, portMAX_DELAY);
    }
  }
}