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

// ===================== WIFI CREDENTIALS =====================
const char* ssid = "Creativa_Banisuef";
const char* password = "12345555";

// ===================== LCD Setup =====================
#define I2C_SDA 21
#define I2C_SCL 22
LiquidCrystal_I2C lcd(0x27, 16, 2);

// ===================== LEDs =====================
const int LED_RED = 4;
const int LED_YELLOW = 16;
const int LED_GREEN = 17;

// ===================== Ultrasonic Sensors =====================
const int SENSOR1_TRIG = 13;
const int SENSOR1_ECHO = 14;

const int SENSOR2_TRIG = 12;
const int SENSOR2_ECHO = 33;

const int SENSOR3_TRIG = 27;
const int SENSOR3_ECHO = 32;

const int SENSOR4_TRIG = 25;
const int SENSOR4_ECHO = 26;

const int SENSOR5_TRIG = 18;
const int SENSOR5_ECHO = 19;

const int SENSOR6_TRIG = 15;
const int SENSOR6_ECHO = 2;

// ===================== Servo =====================
Servo gateServo;
const int SERVO_PIN = 5;

const int OPEN_ANGLE = 0;      // Gate Open (Start)
const int CLOSED_ANGLE = 90;   // Gate Closed
int currentServoAngle = OPEN_ANGLE;

// ===================== Speed Measurement Vars =====================
const float DISTANCE_BETWEEN_SENSORS = 15.0;
bool speedTimingActive = false;
unsigned long speedStartTime = 0;
float measuredSpeed = 0.0;

const long MAX_DETECTION_DURATION_US = 3000;
const long PULSE_TIMEOUT_US = 30000;

// =====================================================
//        BASIC GET DISTANCE (BEGINNER FRIENDLY)
// =====================================================
float getDistance(int trigPin, int echoPin) {

    digitalWrite(trigPin, LOW);
    delayMicroseconds(3);

    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    long duration = pulseIn(echoPin, HIGH, 30000);

    if (duration == 0) return -1;     // No object

    float distanceCM = duration * 0.034 / 2;

    return distanceCM;
}

// =====================================================
//             SIMPLE OBJECT DETECTOR
// =====================================================
bool isObjectDetected(int trigPin, int echoPin) {
    float dist = getDistance(trigPin, echoPin);
    return (dist > 0 && dist < 30);  // car within 30 cm
}

// =====================================================
//           LCD UPDATE
// =====================================================
void updateLCD() {

    lcd.clear();

    lcd.setCursor(0, 0);
    lcd.print("Speed:");
    lcd.print(measuredSpeed, 1);
    lcd.print(" cm/s");

    lcd.setCursor(0, 1);
    lcd.print("Gate:");

    if (currentServoAngle == OPEN_ANGLE)
        lcd.print("OPEN  ");
    else
        lcd.print("CLOSED");
}

// =====================================================
//           SPEED MEASUREMENT LOGIC
// =====================================================
void measureCarSpeed() {

    bool s1 = isObjectDetected(SENSOR1_TRIG, SENSOR1_ECHO);
    bool s2 = isObjectDetected(SENSOR2_TRIG, SENSOR2_ECHO);

    // Start timing
    if (s1 && !speedTimingActive) {
        speedTimingActive = true;
        speedStartTime = millis();
    }

    // Stop timing
    if (speedTimingActive && s2) {

        float timeTaken = (millis() - speedStartTime) / 1000.0;
        if (timeTaken > 0) measuredSpeed = DISTANCE_BETWEEN_SENSORS / timeTaken;

        speedTimingActive = false;
        delay(1000); // prevent double trigger
    }

    // Timeout
    if (speedTimingActive && millis() - speedStartTime > 15000) {
        measuredSpeed = 0;
        speedTimingActive = false;
    }
}

// =====================================================
//           TRAFFIC LED CONTROL
// =====================================================
void controlTrafficLEDs() {

    bool s3 = isObjectDetected(SENSOR3_TRIG, SENSOR3_ECHO);
    bool s4 = isObjectDetected(SENSOR4_TRIG, SENSOR4_ECHO);
    bool s5 = isObjectDetected(SENSOR5_TRIG, SENSOR5_ECHO);

    digitalWrite(LED_RED, LOW);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_GREEN, LOW);

    if (s3 && s4 && s5)
        digitalWrite(LED_GREEN, HIGH);
    else if (s3 && s4)
        digitalWrite(LED_YELLOW, HIGH);
    else if (s5)
        digitalWrite(LED_RED, HIGH);
}

// =====================================================
//         ⭐ FINAL SIMPLE GATE CONTROL LOGIC ⭐
// =====================================================
void controlGateServo() {

    float d = getDistance(SENSOR6_TRIG, SENSOR6_ECHO);

    // Gate OPEN → detect → close → wait → open
    if (d > 0 && d < 30 && currentServoAngle == OPEN_ANGLE) {

        // Close (0 → 90)
        for (int a = OPEN_ANGLE; a <= CLOSED_ANGLE; a++) {
            gateServo.write(a);
            delay(10);
        }
        currentServoAngle = CLOSED_ANGLE;

        delay(15000);   // hold 15 seconds

        // Open (90 → 0)
        for (int a = CLOSED_ANGLE; a >= OPEN_ANGLE; a--) {
            gateServo.write(a);
            delay(10);
        }
        currentServoAngle = OPEN_ANGLE;
    }
}

// =====================================================
//                       SETUP
// =====================================================
void setup() {

    Serial.begin(115200);

    Wire.begin(I2C_SDA, I2C_SCL);
    lcd.init();
    lcd.backlight();

    gateServo.attach(SERVO_PIN);
    gateServo.write(OPEN_ANGLE);

    pinMode(LED_RED, OUTPUT);
    pinMode(LED_YELLOW, OUTPUT);
    pinMode(LED_GREEN, OUTPUT);

    pinMode(SENSOR1_TRIG, OUTPUT); pinMode(SENSOR1_ECHO, INPUT);
    pinMode(SENSOR2_TRIG, OUTPUT); pinMode(SENSOR2_ECHO, INPUT);
    pinMode(SENSOR3_TRIG, OUTPUT); pinMode(SENSOR3_ECHO, INPUT);
    pinMode(SENSOR4_TRIG, OUTPUT); pinMode(SENSOR4_ECHO, INPUT);
    pinMode(SENSOR5_TRIG, OUTPUT); pinMode(SENSOR5_ECHO, INPUT);
    pinMode(SENSOR6_TRIG, OUTPUT); pinMode(SENSOR6_ECHO, INPUT);

    WiFi.begin(ssid, password);
}

// =====================================================
//                        LOOP
// =====================================================
void loop() {

    controlGateServo();
    measureCarSpeed();
    controlTrafficLEDs();
    updateLCD();

    delay(80);
}