#define BLYNK_TEMPLATE_ID "TMPL3tqZ_SP20"
#define BLYNK_TEMPLATE_NAME "smart traffic management"
#define BLYNK_AUTH_TOKEN "h6jjIE6wvfpY3hkZ69yvYYOUknzlJ806"

#include <WiFi.h>
#include <BlynkSimpleEsp32.h>
#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

char auth[] = BLYNK_AUTH_TOKEN;
char ssid[] = "Visvess wifi";
char pass[] = "Harsvish1";

// LCD settings
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Servo settings
Servo divertServo;

// Sensor pins
const int trigPin1 = 32;   // Trig pin of Sensor 1
const int echoPin1 = 33;   // Echo pin of Sensor 1
const int trigPin2 = 34;   // Trig pin of Sensor 2
const int echoPin2 = 35;   // Echo pin of Sensor 2

// LED pins
const int redLED = 16;
const int yellowLED = 17;
const int greenLED = 18;

// Servo positions
const int divertPosition = 90;
const int normalPosition = 0;

// Threshold for traffic detection
const int threshold = 500;

void setup() {
  Serial.begin(115200);
  Blynk.begin(auth, ssid, pass);

  lcd.begin(16, 2);
  lcd.backlight();

  pinMode(redLED, OUTPUT);
  pinMode(yellowLED, OUTPUT);
  pinMode(greenLED, OUTPUT);

  pinMode(trigPin1, OUTPUT); // Trigger pin as output for Sensor 1
  pinMode(echoPin1, INPUT);  // Echo pin as input for Sensor 1
  pinMode(trigPin2, OUTPUT); // Trigger pin as output for Sensor 2
  pinMode(echoPin2, INPUT);  // Echo pin as input for Sensor 2

  divertServo.attach(5);
  divertServo.write(normalPosition);

  lcd.setCursor(0, 0);
  lcd.print("Traffic Control");
  delay(2000);
  lcd.clear();
}

void loop() {
  Blynk.run();

  int traffic1 = readDistance(trigPin1, echoPin1);
  int traffic2 = readDistance(trigPin2, echoPin2);

  if (traffic1 > threshold && traffic2 < threshold) {
    // Divert traffic
    divertTraffic();
  } else {
    // Normal traffic light control
    normalTrafficControl();
  }
}

int readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}

void normalTrafficControl() {
  digitalWrite(redLED, HIGH); // Red LED on
  lcd.setCursor(0, 0);
  lcd.print("STOP!");
  delay(11000);

  digitalWrite(redLED, LOW); // Red LED off
  digitalWrite(yellowLED, HIGH); // Yellow LED on
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Prepare to go!");
  delay(1000);

  digitalWrite(yellowLED, LOW); // Yellow LED off
  digitalWrite(greenLED, HIGH); // Green LED on
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("GO!");
  delay(7000);

  digitalWrite(greenLED, LOW); // Green LED off
  delay(1000); // Transition period
}

void divertTraffic() {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Diverting...");
  divertServo.write(divertPosition);
  delay(5000); // Time for traffic to divert

  divertServo.write(normalPosition);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Normal Traffic");
  delay(2000);
}