#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Initialize the LCD with I2C address 0x27, 16 columns and 2 rows
LiquidCrystal_I2C lcd(0x27, 16, 2);

const int trigPins[] = {33, 25, 27, 19, 15, 4};
const int echoPins[] = {35, 26, 14, 18, 2, 16};

// Variables for the duration of the ping and the distance result
long duration;
float distance;

// Define threshold for "occupied"
const float OCCUPIED_THRESHOLD = 300.0; // Distance threshold in cm

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* hostname = "broker.hivemq.com";

WiFiClient espClient;
PubSubClient client(espClient);
const char* espClientName = "esp32Client_HOONLEE"; 
int PORTNUM = 1883;

void setup_wifi()
{
  WiFi.begin(ssid, password);
  while(WiFi.status() != WL_CONNECTED )
  {
    delay(1000);
    Serial.print(".");
  }
  Serial.println();
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("Given IP by the router to ESP32 is ");
  Serial.println(WiFi.localIP());
}

void connectMQTT()
{
  while(!client.connected() )
  {
    Serial.println("Connecting to MQTT ...");
    if (client.connect(espClientName) ) //, mqttUser, mqttPassword) )
    {
      Serial.println("Connected");

    }
    else
    {
      Serial.print("Failed with state ");
      Serial.print(client.state() );
      delay(1500);
    }
  }
}

void setup_MQTT() 
{
  client.setServer(hostname, PORTNUM);
}

void setup() 
{
  setup_wifi();
  setup_MQTT();

  lcd.init(); // Initialize the LCD
  lcd.backlight(); // Turn on the backlight

  Serial.begin(115200);

  for (int i = 0; i < 6; i++) 
  {
    pinMode(trigPins[i], OUTPUT);
    pinMode(echoPins[i], INPUT);
  }
}

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

  int occupiedCount = 0; // Count of occupied sensors
  bool allSensorsOccupied = true; // Assume all sensors are occupied initially

  for (int i = 0; i < 6; i++) 
  {
    digitalWrite(trigPins[i], LOW);
    delayMicroseconds(2);
    digitalWrite(trigPins[i], HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPins[i], LOW);

    duration = pulseIn(echoPins[i], HIGH);
    distance = (duration * 0.0343) / 2;

    // Output the distance to the serial monitor
    Serial.print("Distance from sensor ");
    Serial.print(i + 1);
    Serial.print(": ");
    Serial.print(distance);
    Serial.println(" cm");

    // Check if sensor distance is less than threshold
   if (distance < OCCUPIED_THRESHOLD) 
    {
      occupiedCount++; // Increment occupied count
    } 
    else 
    {
     allSensorsOccupied = false; // If any sensor is not occupied, set to false
    }
  }

  // Display the status on the LCD
  lcd.clear(); // Clear the LCD screen

  // Display available count on the first row
  lcd.setCursor(0, 0); // Set cursor to the first line
  lcd.print(6 - occupiedCount); // Display the number of available sensors

  // Display occupied/available status on the second row
  lcd.setCursor(0, 1); // Set cursor to the second line
    if (allSensorsOccupied) 
    {
     lcd.print("Occupied");
    } 
    else 
    {
      lcd.print("Available");
    }

    // Publish to MQTT
  char message[20];
  sprintf(message, "%d", 6 - occupiedCount);
  client.publish("available_sensors", message);

  if (allSensorsOccupied) 
  {
    client.publish("CarPark_status", "Occupied");
  } 
  else 
  {
    client.publish("CarPark_status", "Available");
  }

  delay(500); // Add a delay to avoid overlapping readings
}