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

// --- PIN DEFINITIONS ---
#define TRIG_IN_PIN    5
#define ECHO_IN_PIN    18
#define TRIG_OUT_PIN   4
#define ECHO_OUT_PIN   19
#define SERVO_GATE_PIN 13
#define BUZZER_PIN     27
#define LED_GREEN_PIN  25
#define LED_RED_PIN    26

// --- THRESHOLDS & CAPACITIES ---
const int BRIDGE_CAPACITY    = 20;  // Maximum rated capacity for simulation
const int THRESHOLD_WARN     = 12;  // 60% Warning mark
const int THRESHOLD_CRUSH    = 17;  // 85% Critical safety threshold
const int DETECTION_DIST_CM  = 50;  // Detection barrier threshold in cm

// --- SYSTEM STATE VARIABLES ---
int occupancy = 0;
int totalInflow = 0;
int totalOutflow = 0;
int recentOutflowWindow = 0;
unsigned long lastStagnationCheck = 0;
unsigned long lastMqttPublish = 0;

bool inDetectedPrev = false;
bool outDetectedPrev = false;

enum SystemState { SAFE, WARNING, CRUSH_EMERGENCY };
SystemState currentState = SAFE;

// --- PERIPHERALS & NETWORKING ---
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo reliefGate;
WiFiClient espClient;
PubSubClient mqttClient(espClient);

// Public MQTT broker (Free, open, zero registration needed)
const char* mqttServer = "broker.hivemq.com";
const int   mqttPort   = 1883;
// Unique topic name:
const char* topicTelemetry = "city/fob_safety/telemetry";
const char* topicAlerts    = "city/fob_safety/emergency";

// --- ULTRASONIC RANGE HELPER ---
long readDistanceCM(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000); // 30ms timeout
  if (duration == 0) return 999;                 // No echo
  return duration * 0.034 / 2;
}

// --- CLOUD SETUP ---
void connectWiFi() {
  WiFi.mode(WIFI_STA);
  WiFi.begin("Wokwi-GUEST", ""); // Wokwi simulated WiFi AP
  Serial.print("Connecting to Virtual WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(300);
    Serial.print(".");
  }
  Serial.println("\n[CONNECTED] IP Assigned: " + WiFi.localIP().toString());
}

void reconnectMQTT() {
  while (!mqttClient.connected()) {
    Serial.print("Attempting MQTT connection...");
    String clientId = "ESP32_BridgeNode_" + String(random(0xffff), HEX);
    if (mqttClient.connect(clientId.c_str())) {
      Serial.println("CONNECTED to HiveMQ Broker!");
      mqttClient.publish(topicAlerts, "{\"system\":\"ONLINE\",\"node_id\":\"FOB_01\"}");
    } else {
      Serial.print("Failed, rc=");
      Serial.print(mqttClient.state());
      Serial.println(" Retrying in 2 seconds...");
      delay(2000);
    }
  }
}

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

  pinMode(TRIG_IN_PIN, OUTPUT);
  pinMode(ECHO_IN_PIN, INPUT);
  pinMode(TRIG_OUT_PIN, OUTPUT);
  pinMode(ECHO_OUT_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_GREEN_PIN, OUTPUT);
  pinMode(LED_RED_PIN, OUTPUT);

  // Initialize Servo
  reliefGate.attach(SERVO_GATE_PIN);
  reliefGate.write(0); // 0 deg = Gate Closed (Normal)

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("FOB CROWD SENTRY");
  lcd.setCursor(0, 1);
  lcd.print("Init Sensors...");

  connectWiFi();
  mqttClient.setServer(mqttServer, mqttPort);

  delay(1500);
  lcd.clear();
}

void loop() {
  if (!mqttClient.connected()) {
    reconnectMQTT();
  }
  mqttClient.loop();

  // --- 1. SENSOR READINGS & FLOW COUNTING ---
  long distIn = readDistanceCM(TRIG_IN_PIN, ECHO_IN_PIN);
  long distOut = readDistanceCM(TRIG_OUT_PIN, ECHO_OUT_PIN);

  // Detect edge: Entrance trigger
  bool inDetected = (distIn < DETECTION_DIST_CM);
  if (inDetected && !inDetectedPrev) {
    totalInflow++;
    occupancy++;
    Serial.println("[FLOW] Pedestrian Entered. Current Occupancy: " + String(occupancy));
  }
  inDetectedPrev = inDetected;

  // Detect edge: Exit trigger
  bool outDetected = (distOut < DETECTION_DIST_CM);
  if (outDetected && !outDetectedPrev) {
    if (occupancy > 0) occupancy--;
    totalOutflow++;
    recentOutflowWindow++;
    Serial.println("[FLOW] Pedestrian Exited. Current Occupancy: " + String(occupancy));
  }
  outDetectedPrev = outDetected;

  // --- 2. CROWD STAGNATION FACTOR (CSF) EVALUATION ---
  // Evaluates every 10 seconds if people are entering while outflow is stalled
  if (millis() - lastStagnationCheck >= 10000) {
    if (occupancy >= THRESHOLD_WARN && recentOutflowWindow == 0) {
      Serial.println("[WARNING] STAGNATION DETECTED: Inflow occurring with ZERO outflow!");
    }
    recentOutflowWindow = 0;
    lastStagnationCheck = millis();
  }

  // --- 3. FINITE STATE MACHINE (FSM) LOGIC ---
  if (occupancy >= THRESHOLD_CRUSH) {
    currentState = CRUSH_EMERGENCY;
  } else if (occupancy >= THRESHOLD_WARN) {
    currentState = WARNING;
  } else {
    currentState = SAFE;
  }

  // --- 4. ACTUATION & USER INTERFACE ---
  switch (currentState) {
    case SAFE:
      digitalWrite(LED_GREEN_PIN, HIGH);
      digitalWrite(LED_RED_PIN, LOW);
      noTone(BUZZER_PIN);
      reliefGate.write(0); // Bypass Gate Closed

      lcd.setCursor(0, 0);
      lcd.print("SAFE  Occ:" + String(occupancy) + "/" + String(BRIDGE_CAPACITY) + "  ");
      lcd.setCursor(0, 1);
      lcd.print("In:" + String(totalInflow) + " Out:" + String(totalOutflow) + "    ");
      break;

    case WARNING:
      // Alternating warning blink
      digitalWrite(LED_GREEN_PIN, (millis() / 300) % 2 == 0);
      digitalWrite(LED_RED_PIN, (millis() / 300) % 2 != 0);
      
      // Gentle audible pulse
      if ((millis() / 400) % 2 == 0) {
        tone(BUZZER_PIN, 1000);
      } else {
        noTone(BUZZER_PIN);
      }
      reliefGate.write(0); // Gate stays closed, warning issued

      lcd.setCursor(0, 0);
      lcd.print("WARN! Occ:" + String(occupancy) + "/" + String(BRIDGE_CAPACITY) + "  ");
      lcd.setCursor(0, 1);
      lcd.print("SLOW DOWN/WAIT  ");
      break;

    case CRUSH_EMERGENCY:
      digitalWrite(LED_GREEN_PIN, LOW);
      digitalWrite(LED_RED_PIN, HIGH);
      tone(BUZZER_PIN, 2400); // Continuous shrill alarm

      // Open emergency bypass gate to 90 degrees
      reliefGate.write(90);

      lcd.setCursor(0, 0);
      lcd.print("CRUSH DANGER!   ");
      lcd.setCursor(0, 1);
      lcd.print("BYPASS OPEN 90D ");
      break;
  }

  // --- 5. MQTT TELEMETRY & CLOUD BROADCAST ---
  if (millis() - lastMqttPublish >= 3000) { // Every 3 seconds
    lastMqttPublish = millis();

    String telemetryJson = "{";
    telemetryJson += "\"occupancy\":" + String(occupancy) + ",";
    telemetryJson += "\"capacity\":" + String(BRIDGE_CAPACITY) + ",";
    telemetryJson += "\"inflow\":" + String(totalInflow) + ",";
    telemetryJson += "\"outflow\":" + String(totalOutflow) + ",";
    telemetryJson += "\"status\":\"" + String(currentState == CRUSH_EMERGENCY ? "EMERGENCY" : (currentState == WARNING ? "WARNING" : "NORMAL")) + "\",";
    telemetryJson += "\"bypass_gate_open\":" + String(currentState == CRUSH_EMERGENCY ? "true" : "false");
    telemetryJson += "}";

    mqttClient.publish(topicTelemetry, telemetryJson.c_str());

    if (currentState == CRUSH_EMERGENCY) {
      String alertJson = "{\"ALERT\":\"CRUSH_FORCE_DETECTED\",\"occupancy\":" + String(occupancy) + ",\"action\":\"BYPASS_EVAC_OPEN\"}";
      mqttClient.publish(topicAlerts, alertJson.c_str());
      Serial.println("[MQTT EMERGENCY BROADCAST SENT]");
    }
  }

  delay(50);
}