#include <Servo.h>
#include <Tiny4kOLED.h>

// Définir les pins des capteurs à ultrasons
const int trigPin1 = 2;
const int echoPin1 = 3;
const int trigPin2 = 4;
const int echoPin2 = 5;

// Définir les pins des feux de circulation (Feu 1 et Feu 2)
const int redLed1 = 6, orangeLed1 = 7, greenLed1 = 8;
const int redLed2 = 9, orangeLed2 = 10, greenLed2 = 11;

// Initialisation des servomoteurs
Servo barrier1;
Servo barrier2;

// Définir les pins des servomoteurs
const int servoPin1 = 12;
const int servoPin2 = 13;

// Variables pour les distances mesurées
float distance1 = 0;
float distance2 = 0;

void setup() {
  Serial.begin(9600);

  // Configurer les pins des capteurs à ultrasons
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  // Configurer les pins des LEDs
  pinMode(redLed1, OUTPUT);
  pinMode(orangeLed1, OUTPUT);
  pinMode(greenLed1, OUTPUT);
  pinMode(redLed2, OUTPUT);
  pinMode(orangeLed2, OUTPUT);
  pinMode(greenLed2, OUTPUT);

  // Initialiser les servomoteurs
  barrier1.attach(servoPin1);
  barrier2.attach(servoPin2);

  // Fermer les barrières au départ
  barrier1.write(0); // Barrière fermée
  barrier2.write(0); // Barrière fermée

  // Initialiser l'écran OLED
  oled.begin();
  oled.clear();
  oled.on();
  oled.setFont(FONT6X8);
}

float measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH) * 0.034 / 2;
}

void displayStatus(String status) {
  oled.clear();
  oled.setCursor(0, 0);
  oled.print("Traffic System");
  oled.setCursor(0, 2);
  oled.print(status);
  
}

void loop() {
  // Mesurer la distance pour chaque capteur
  distance1 = measureDistance(trigPin1, echoPin1);
  distance2 = measureDistance(trigPin2, echoPin2);

  // Contrôler le feu de circulation 1
  if (distance1 < 50) {
    // Voiture proche du feu 1
    digitalWrite(greenLed1, HIGH);
    digitalWrite(redLed1, LOW);
    barrier1.write(90); // Ouvrir la barrière
    displayStatus("Feu 1: Vert, Barriere 1: Ouverte");
    delay(5000); // Garder la barrière ouverte quelques secondes
    digitalWrite(greenLed1, LOW);
    digitalWrite(redLed1, HIGH);
    barrier1.write(0); // Fermer la barrière
  } else {
    // Aucun véhicule proche
    digitalWrite(redLed1, HIGH);
    digitalWrite(greenLed1, LOW);
    barrier1.write(0); // Garder la barrière fermée
  }

  // Contrôler le feu de circulation 2
  if (distance2 < 50) {
    // Voiture proche du feu 2
    digitalWrite(greenLed2, HIGH);
    digitalWrite(redLed2, LOW);
    barrier2.write(90); // Ouvrir la barrière
    displayStatus("Feu 2: Vert, Barriere 2: Ouverte");
    delay(5000); // Garder la barrière ouverte quelques secondes
    digitalWrite(greenLed2, LOW);
    digitalWrite(redLed2, HIGH);
    barrier2.write(0); // Fermer la barrière
  } else {
    // Aucun véhicule proche
    digitalWrite(redLed2, HIGH);
    digitalWrite(greenLed2, LOW);
    barrier2.write(0); // Garder la barrière fermée
  }

  // Ajouter un délai pour éviter une lecture excessive
  delay(100);
}