#define BLYNK_TEMPLATE_ID "TMPL2l_tOebCw"
#define BLYNK_TEMPLATE_NAME "Bin Storage State"
#define BLYNK_AUTH_TOKEN "yI3AOkEwAIz6AfTXGyIY9aSgAd6xJEUN"

#define BLYNK_PRINT Serial
#include <ESP32Servo.h>
#include <WiFi.h>
#include <BlynkSimpleEsp32.h>
#include "PubSubClient.h"

#define TRIG_PIN 18  // GPIO pin connected to TRIG of HC-SR04
#define ECHO_PIN 19  // GPIO pin connected to ECHO of HC-SR04
#define TRIG_PIN_INSIDE 12
#define ECHO_PIN_INSIDE 14
#define LED 21

const char* mqtt_server = "broker.emqx.io";
const int mqtt_port = 1883 ;
const char* lid_topic = "smartbin/lid";
const char* status_topic = "smartbin/status";

WiFiClient espClient;
PubSubClient client(espClient);


int pos = 0, speed = 15;
bool lidOpen = false;
Servo myServo;  // Create a Servo object

// WiFi credentials
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

// Function declarations
long getUltrasonicDistance(int trig, int echo);
void open();
void close();
boolean isFull();
void updateStatus();
void connectToMQTT();
void connectToWIFI();
void mqttCallback(char* topic, byte* payload, unsigned int length);


BlynkTimer timer;

void setup() {
  Serial.begin(115200);
  myServo.attach(5);

  // Connect to WiFi and Blynk 
  connectToWIFI();
  
  // Connect to MQTT
  client.setServer(mqtt_server,mqtt_port);
  client.setCallback(callback);
  connectToMQTT();  
  client.subscribe(lid_topic);
  Serial.println("Subscribed to topic: smartbin/lid");

  //Connect to Blynk
  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);
  Blynk.virtualWrite(V0,0);  
  connectToWIFI();
  Blynk.virtualWrite(V1, "Loading...");
  BLYNK_WRITE(V2);

  // Setup pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN_INSIDE, OUTPUT);
  pinMode(ECHO_PIN_INSIDE, INPUT);
  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);

  // Setup timer to update status every 5 seconds
  //timer.setInterval(5000L, updateStatus);
  Serial.println("********************Setup complete");

}

void loop() {
  Blynk.run();
  timer.run();
  if (WiFi.status() != WL_CONNECTED){
    connectToWIFI();
  }

  if (!Blynk.connect()){
    connectToBLYNK();
  }
  if (!client.connected()) {
    connectToMQTT();
  }

  client.loop();
  BLYNK_WRITE(V2);
  updateStatus();

  // Monitor for automatic lid open/close
  float distance = getUltrasonicDistance(TRIG_PIN, ECHO_PIN);
  if (distance < 16 && !isFull()) 
  {
    open();
  } 
  // if full , the user opens it through the button in blynk app or alexa
  else if(isFull()) 
  {
    Serial.println("BIN IS FULL");
    digitalWrite(LED, HIGH);
    connectToWIFI();
    Blynk.virtualWrite(V1, "Full");
    Blynk.logEvent("bin_full__");
  } 
  else {
    digitalWrite(LED, LOW);
    connectToWIFI();
    Blynk.virtualWrite(V1, "Not Full");
  }

  Serial.print("Outside Distance: ");
  Serial.println(distance);

  
  delay(1000);
}

long getUltrasonicDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  return pulseIn(echo, HIGH) * 0.0343 / 2; 
}

void open() {
  if (!lidOpen) {
    while (pos <= 89) {
      pos += 1;
      myServo.write(pos);
      delay(speed);
    }
    lidOpen = true;
    delay(5000);
    close(); 
  }

}

void close() {
  if (lidOpen) {
    while (pos >= 1) {
      pos -= 1;
      myServo.write(pos);
      delay(speed);
      lidOpen = false;
    }
  }
}

boolean isFull() {
  float distance = getUltrasonicDistance(TRIG_PIN_INSIDE, ECHO_PIN_INSIDE);
  Serial.print("Distance inside bin: ");
  Serial.println(distance);
  return (distance < 8);
}

void updateStatus() {
  float distance = getUltrasonicDistance(TRIG_PIN_INSIDE, ECHO_PIN_INSIDE);
  int height = 40;  // Height of the bin in cm
  float percentage = ((height - distance) / height) * 100;

  if (percentage < 0) percentage = 0;

  if (percentage >= 90 ) {
    digitalWrite(LED,HIGH);
    Blynk.logEvent("bin_full__");
  }

  // Send percentage to Blynk and MQTT 
  String statusMessage = String(percentage) + "% full";
  Serial.print("Status Message: ");
  Serial.println(statusMessage);
  connectToMQTT();
  client.publish(status_topic, statusMessage.c_str());
  Blynk.virtualWrite(V0, percentage);  
}

// Blynk button to manually control the lid
BLYNK_WRITE(V2) {  
  int buttonState = param.asInt();
  if (buttonState == 1) {  // Button pressed
    open();
    lidOpen = true;
    Serial.println("Button Pressed");
  } else {  // Button released
    close();
    lidOpen = false;
  }
}


//---------------------------------------------MQTT--------------------------------------------------------------------------

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

  if (String(topic) == "smartbin/lid") {
    Serial.print("Changing output to ");
    if(stMessage == "open"){
      open();
      Serial.println("Lid is Opened");   
      close();
      delay(1000);      // Short delay to ensure lid closes
      Serial.println("Lid is Closed");
    }

  }
}


void connectToMQTT() {
  while (!client.connected()) {
    if (client.connect("ESP32Client")) { 
      Serial.println("------------MQTT Connected!");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" retrying in 4 seconds");
      delay(4000);
    }
  }
}


void connectToWIFI()
{
  WiFi.begin(ssid, pass ,6);
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }
  Serial.println(" ----------------WiFi Connected!");
}

void connectToBLYNK()
{
  while (!Blynk.connect()){
    Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);
  }
}