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

// ===================== WiFi Settings =====================
const char* ssid = "WED7742C";
const char* password = "aa0e323b";
const char* googleScriptURL = "https://script.google.com/macros/s/AKfycbwWj6PH244jSQKqG-Yv6wj9ajUpLsLBwk6-JMvfopqHjLMkdw3Ll1JAYuLkqkR6csmEsw/exec";

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

// ===================== LED Pins =====================
const int LED_RED = 4;
const int LED_YELLOW = 16;
const int LED_GREEN = 17;

// ===================== Ultrasonic Sensor Pins =====================
// Ultrasonic 1 - Speed measurement START
const int SENSOR1_TRIG = 5;
const int SENSOR1_ECHO = 18;

// Ultrasonic 2 - Speed measurement END
const int SENSOR2_TRIG = 19;
const int SENSOR2_ECHO = 23;

// Ultrasonic 3 - Traffic detection 1
const int SENSOR3_TRIG = 25;
const int SENSOR3_ECHO = 26;

// Ultrasonic 4 - Traffic detection 2
const int SENSOR4_TRIG = 27;
const int SENSOR4_ECHO = 14;

// Ultrasonic 5 - Traffic detection 3
const int SENSOR5_TRIG = 12;
const int SENSOR5_ECHO = 13;

// Ultrasonic 6 - Gate control
const int SENSOR6_TRIG = 15;
const int SENSOR6_ECHO = 32;

// ===================== Servo Setup =====================
Servo gateServo;
const int SERVO_PIN = 13;
int currentServoAngle = 0;

// ===================== Speed Measurement Variables =====================
const float DISTANCE_BETWEEN_SENSORS = 15.0;
const int DETECTION_THRESHOLD = 20;

bool speedTimingActive = false;
unsigned long speedStartTime = 0;
float measuredSpeed = 0.0;

// ===================== FUNCTION: Read Distance from Ultrasonic Sensor =====================
float getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000);
  
  if (duration == 0) {
    return -1;
  }
  
  float distance = duration * 0.034 / 2;
  return distance;
}

// ===================== FUNCTION: Measure Car Speed =====================
void measureCarSpeed() {
  float distance1 = getDistance(SENSOR1_TRIG, SENSOR1_ECHO);

  if (distance1 > 0 && distance1 < DETECTION_THRESHOLD && !speedTimingActive) {
    speedTimingActive = true;
    speedStartTime = millis();
    Serial.println("Speed measurement started!");
  }

  if (speedTimingActive) {
    float distance2 = getDistance(SENSOR2_TRIG, SENSOR2_ECHO);
    
    if (distance2 > 0 && distance2 < DETECTION_THRESHOLD) {
      float timeTaken = (millis() - speedStartTime) / 1000.0;
      
      if (timeTaken > 0) {
        measuredSpeed = DISTANCE_BETWEEN_SENSORS / timeTaken;
        
        Serial.print("Speed measured: ");
        Serial.print(measuredSpeed);
        Serial.println(" cm/s");
        
        sendSpeedToGoogle(measuredSpeed);
      }
      
      speedTimingActive = false;
      delay(2000);
    }

    if (millis() - speedStartTime > 10000) {
      speedTimingActive = false;
      Serial.println("Speed measurement timeout");
    }
  }
}

// ===================== FUNCTION: Control Traffic LEDs =====================
void controlTrafficLEDs() {
  float distance3 = getDistance(SENSOR3_TRIG, SENSOR3_ECHO);
  float distance4 = getDistance(SENSOR4_TRIG, SENSOR4_ECHO);
  float distance5 = getDistance(SENSOR5_TRIG, SENSOR5_ECHO);

  bool sensor3_detected = (distance3 > 0 && distance3 < DETECTION_THRESHOLD);
  bool sensor4_detected = (distance4 > 0 && distance4 < DETECTION_THRESHOLD);
  bool sensor5_detected = (distance5 > 0 && distance5 < DETECTION_THRESHOLD);

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

  if (sensor3_detected && sensor4_detected && sensor5_detected) {
    digitalWrite(LED_GREEN, HIGH);
    Serial.println("GREEN LED ON (All sensors detect)");
  }
  else if (sensor3_detected && sensor4_detected) {
    digitalWrite(LED_YELLOW, HIGH);
    Serial.println("YELLOW LED ON (Sensors 3 & 4 detect)");
  }
  else if (sensor5_detected) {
    digitalWrite(LED_RED, HIGH);
    Serial.println("RED LED ON (Sensor 5 detects)");
  }
}

// ===================== FUNCTION: Control Gate Servo =====================
void controlGateServo() {
  float distance6 = getDistance(SENSOR6_TRIG, SENSOR6_ECHO);

  int targetAngle;

  if (distance6 > 0 && distance6 < DETECTION_THRESHOLD) {
    targetAngle = 90;
  } 
  else {
    targetAngle = 0;
  }

  if (currentServoAngle != targetAngle) {
    if (currentServoAngle < targetAngle) {
      for (int angle = currentServoAngle; angle <= targetAngle; angle++) {
        gateServo.write(angle);
        delay(15);
      }
    } else {
      for (int angle = currentServoAngle; angle >= targetAngle; angle--) {
        gateServo.write(angle);
        delay(15);
      }
    }
    
    currentServoAngle = targetAngle;
    Serial.print("Gate moved to: ");
    Serial.println(currentServoAngle == 0 ? "OPEN" : "CLOSED");
  }
}

// ===================== FUNCTION: Update LCD Display =====================
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 == 0) {
    lcd.print("OPEN   ");
  } else {
    lcd.print("CLOSED ");
  }
}

// ===================== FUNCTION: Send Speed to Google Sheets (FIXED) =====================
void sendSpeedToGoogle(float speed) {
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("WiFi not connected, cannot send data");
    return;
  }

  HTTPClient http;
  
  String url = String(googleScriptURL) + "?speed=" + String(speed, 2);
  
  Serial.println("Sending to Google Sheets...");
  Serial.println(url);
  
  // Enable redirect following
  http.begin(url);
  http.setFollowRedirects(HTTPC_STRICT_FOLLOW_REDIRECTS);
  http.setTimeout(10000);
  
  int httpCode = http.GET();
  
  if (httpCode > 0) {
    Serial.print("HTTP Response Code: ");
    Serial.println(httpCode);
    
    if (httpCode == HTTP_CODE_OK || httpCode == HTTP_CODE_MOVED_PERMANENTLY || httpCode == HTTP_CODE_FOUND) {
      String response = http.getString();
      Serial.println("Response:");
      Serial.println(response);
      
      if (response.indexOf("Success") >= 0 || response.indexOf("success") >= 0) {
        Serial.println("✓ Data sent successfully!");
      } else {
        Serial.println("✗ Unexpected response");
      }
    }
  } else {
    Serial.print("✗ HTTP Error: ");
    Serial.println(http.errorToString(httpCode));
  }
  
  http.end();
}

// ===================== SETUP (Runs Once) =====================
void setup() {
  Serial.begin(115200);
  Serial.println("ESP32 Traffic Control System Starting...");

  Serial.print("Connecting to WiFi");
  WiFi.begin(ssid, password);
  
  int wifiTimeout = 0;
  while (WiFi.status() != WL_CONNECTED && wifiTimeout < 20) {
    delay(500);
    Serial.print(".");
    wifiTimeout++;
  }
  
  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("\nWiFi Connected!");
    Serial.print("IP Address: ");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println("\nWiFi Connection Failed - continuing without internet");
  }

  Wire.begin(I2C_SDA, I2C_SCL);
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.print("System Ready!");
  delay(2000);

  ESP32PWM::allocateTimer(0);
  gateServo.attach(SERVO_PIN);
  gateServo.write(currentServoAngle);
  Serial.println("Servo initialized - Gate OPEN");

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

  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);

  Serial.println("All sensors initialized");
  Serial.println("System running...\n");
}

// ===================== MAIN LOOP (Runs Continuously) =====================
void loop() {
  measureCarSpeed();
  controlTrafficLEDs();
  controlGateServo();
  updateLCD();
  delay(100);
}