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

// Replace with your WiFi and MQTT credentials
#define WIFI_SSID "Wokwi-GUEST"
#define WIFI_PASSWORD ""
const char* mqttServer = "broker.emqx.io";
const int mqttPort = 1883;

// MQTT topic and device ID
const char* topic = "parking/status";
const char* deviceID = "PicoW_441801";

// Pin Definitions for Raspberry Pi Pico W
const int sonarTrigPin = 5;   // GPIO pin for Sonar Trigger
const int sonarEchoPin = 18;  // GPIO pin for Sonar Echo
const int greenLEDPin = 23;   // GPIO pin for Green LED
const int redLEDPin = 22;     // GPIO pin for Red LED

// Threshold distance in cm to consider the space occupied
const int thresholdDistance = 20;

// Initialize WiFi and MQTT client
WiFiClient espClient;
PubSubClient client(espClient);

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

  // Initialize pins
  pinMode(sonarTrigPin, OUTPUT);
  pinMode(sonarEchoPin, INPUT);
  pinMode(greenLEDPin, OUTPUT);
  pinMode(redLEDPin, OUTPUT);

  // Initialize LEDs to be off
  digitalWrite(greenLEDPin, LOW);
  digitalWrite(redLEDPin, LOW);

  // Connect to Wi-Fi
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected to WiFi");

  // Set MQTT server and connect
  client.setServer(mqttServer, mqttPort);
  connectMQTT();
}

void connectMQTT() {
  while (!client.connected()) {
    Serial.print("Connecting to MQTT...");
    if (client.connect(deviceID)) {
      Serial.println("Connected to MQTT");
    } else {
      Serial.print("Failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

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

  long duration;
  int distance;

  // Trigger the Sonar sensor
  digitalWrite(sonarTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(sonarTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(sonarTrigPin, LOW);

  duration = pulseIn(sonarEchoPin, HIGH);
  distance = duration * 0.0344 / 2;

  // Determine the parking space status
  bool isOccupied = distance < thresholdDistance;

  // Update LED status
  digitalWrite(greenLEDPin, !isOccupied); // Green ON when space is available
  digitalWrite(redLEDPin, isOccupied);    // Red ON when space is occupied

  // Prepare and send data
  StaticJsonDocument<200> jsonDoc;
  jsonDoc["deviceID"] = deviceID;
  jsonDoc["timestamp"] = millis();
  jsonDoc["distance"] = distance;
  jsonDoc["status"] = isOccupied ? "Occupied" : "Available";

  String payload;
  serializeJson(jsonDoc, payload);

  client.publish(topic, payload.c_str());

  delay(5000); // Wait before the next measurement
}