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

// Definisi pin untuk Ultrasonik
#define TRIG_PIN 23
#define ECHO_PIN 22

// Definisi pin untuk LED RGB
#define RED_PIN 18
#define GREEN_PIN 19
#define BLUE_PIN 21

// Definisi WiFi dan MQTT
const char* ssid = "Wokwi-GUEST";          // Ganti dengan SSID WiFi Anda
const char* password = "";  // Ganti dengan password WiFi Anda
const char* mqtt_server = "broker.emqx.io";  // Broker EMQX publik
const char* publish_topic = "sensor/tongSampah/jarak";  // Topik untuk publish data jarak

WiFiClient espClient;
PubSubClient client(espClient);

// Fungsi untuk menghubungkan ke WiFi
void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Menghubungkan ke ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

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

  Serial.println();
  Serial.println("WiFi terhubung");
  Serial.println("Alamat IP: ");
  Serial.println(WiFi.localIP());
}

// Fungsi untuk menghubungkan ke broker MQTT
void reconnect() {
  while (!client.connected()) {
    Serial.print("Mencoba menyambung ke MQTT Broker...");
    digitalWrite(RED_PIN, HIGH);  // LED merah nyala, tidak terhubung

    if (client.connect("ESP32Client")) {
      Serial.println("Terhubung ke broker MQTT");
      digitalWrite(RED_PIN, LOW);   // Matikan LED merah
      digitalWrite(GREEN_PIN, HIGH);  // LED hijau nyala, terhubung
    } else {
      Serial.print("Gagal menyambung, rc=");
      Serial.print(client.state());
      Serial.println(" mencoba lagi dalam 5 detik");
      delay(5000);
    }
  }
}

// Fungsi untuk membaca jarak dari sensor ultrasonik
long readUltrasonicDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH);
  long distance = duration * 0.034 / 2;  // Konversi waktu ke jarak dalam cm
  return distance;
}

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

  // Inisialisasi pin Ultrasonik
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Inisialisasi pin LED RGB
  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN, OUTPUT);

  // Matikan semua LED di awal
  digitalWrite(RED_PIN, LOW);
  digitalWrite(GREEN_PIN, LOW);
  digitalWrite(BLUE_PIN, LOW);

  // Inisialisasi koneksi WiFi
  setup_wifi();

  // Set MQTT server
  client.setServer(mqtt_server, 1883);
}

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

  // Membaca jarak dari sensor ultrasonik
  long distance = readUltrasonicDistance();
  Serial.print("Jarak: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Mengirim jarak ke broker MQTT jika jarak kurang dari 50 cm
  if (distance < 50) {
    char msg[50];
    snprintf(msg, 50, "Jarak: %ld cm", distance);
    Serial.print("Mengirim pesan: ");
    Serial.println(msg);

    // Indikator LED biru nyala saat mengirim data
    digitalWrite(BLUE_PIN, HIGH);
    client.publish(publish_topic, msg);
    delay(500);  // Tunggu sebentar agar LED biru bisa terlihat
    digitalWrite(BLUE_PIN, LOW);
  }

  delay(1000);  // Tunggu 1 detik sebelum pembacaan selanjutnya
}