#include <ESP32Servo.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h> // Include MQTT library

const char* ssid = "YourWiFiSSID";
const char* password = "YourWiFiPassword";

const char* mqtt_server = "mqtt-dashboard.com"; // MQTT broker server
const int mqtt_port = 1883; // MQTT port (usually 1883)

const char* mqtt_user = "YourMQTTUsername"; // MQTT username
const char* mqtt_password = "YourMQTTPassword"; // MQTT password

const char* mqtt_topic_distance = "smart_bin/distance"; // MQTT topic for distance data
const char* mqtt_topic_lid_status = "smart_bin/lid_status"; // MQTT topic for lid status

const int ServoPin = 2;
const int ledPin2 = 14;
const int PIN_TRIG = 13;
const int PIN_ECHO = 12;
const int speed = 30;
int lidState = LOW;
int pos = 90;
int threshold = 15;

WiFiClient espClient;
PubSubClient client(espClient);

Servo servo;

void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  for (int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
  }
  Serial.println();

  // Convert payload to integer
  int value = atoi((char*)payload);

  // Process the message based on topic
  if (strcmp(topic, mqtt_topic_lid_status) == 0) {
    if (value == 1) {
      open();
    } else if (value == 0) {
      close();
    }
  }
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("ESP32Client", mqtt_user, mqtt_password)) {
      Serial.println("connected");
      // Once connected, subscribe to topics
      client.subscribe(mqtt_topic_lid_status);
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  servo.attach(ServoPin, 500, 2400);
  pinMode(ledPin2, OUTPUT);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  setup_wifi();
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);
}

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

  // Measure distance
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  int duration = pulseIn(PIN_ECHO, HIGH);
  int distance = duration / 58;

  // Publish distance to MQTT topic
  client.publish(mqtt_topic_distance, String(distance).c_str());

  // Check if bin is full
  bool is_full = distance <= threshold;

  if (is_full) {
    digitalWrite(ledPin2, HIGH);
  } else {
    digitalWrite(ledPin2, LOW);
  }

  delay(1000);
}

void open() {
  if (lidState == LOW) {
    while (pos >= 1) {
      servo.write(pos);
      pos -= 1;
      delay(speed);
    }
    lidState = HIGH;
  }
}

void close() {
  if (lidState == HIGH) {
    while (pos <= 89) {
      servo.write(pos);
      pos += 1;
      delay(speed);
    }
    lidState = LOW;
  }
}