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

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "broker.hivemq.com";
const int mqtt_port = 1883;
const char* mqtt_topic = "parking/update";

#define TRIG1 5
#define ECHO1 18
#define TRIG2 19
#define ECHO2 21
#define TRIG3 23
#define ECHO3 22
#define SERVO_PIN 27
const int THRESHOLD = 10;

WiFiClient espClient;
PubSubClient client(espClient);

void connectMQTT() {
  while (!client.connected()) {
    if (client.connect("ESP32ParkingClient")) {
      Serial.println("MQTT connected!");
    } else {
      delay(2000);
    }
  }
}

long readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH, 30000);
  return duration * 0.034 / 2;
}

void setServoAngle(int angle) {
  int duty = map(angle, 0, 180, 1638, 8192);
  ledcWrite(SERVO_PIN, duty);
}

void setup() {
  Serial.begin(115200);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.println("Connecting to WiFi...");
  }
  Serial.println("WiFi connected!");
  client.setServer(mqtt_server, mqtt_port);
  connectMQTT();
  pinMode(TRIG1, OUTPUT); pinMode(ECHO1, INPUT);
  pinMode(TRIG2, OUTPUT); pinMode(ECHO2, INPUT);
  pinMode(TRIG3, OUTPUT); pinMode(ECHO3, INPUT);
  ledcAttach(SERVO_PIN, 50, 16);
  setServoAngle(0);
}

void loop() {
  if (!client.connected()) connectMQTT();
  client.loop();

  long d1    = readDistance(TRIG1, ECHO1);
  long d2    = readDistance(TRIG2, ECHO2);
  long dGate = readDistance(TRIG3, ECHO3);

  String status1 = (d1 < THRESHOLD) ? "occupied" : "free";
  String status2 = (d2 < THRESHOLD) ? "occupied" : "free";

  String msg1 = "{\"placeId\":1,\"status\":\"" + status1 + "\"}";
  String msg2 = "{\"placeId\":2,\"status\":\"" + status2 + "\"}";
  client.publish(mqtt_topic, msg1.c_str());
  client.publish(mqtt_topic, msg2.c_str());

  Serial.println("Published: " + msg1);
  Serial.println("Published: " + msg2);

  bool place1Free = (d1 >= THRESHOLD);
  bool place2Free = (d2 >= THRESHOLD);
  bool carAtGate  = (dGate < THRESHOLD);

  if ((place1Free || place2Free) && carAtGate) {
    setServoAngle(90);
    Serial.println("Barriere: OUVERTE");
  } else {
    setServoAngle(0);
    Serial.println("Barriere: FERMEE");
  }

  delay(3000);
}