#include <WiFi.h>
#include <MQTT.h>
#include <ESP32Servo.h>

// Konfigurasi WiFi
const char* ssid = "Wokwi-GUEST";         
const char* password = ""; 

// Konfigurasi MQTT
// const char* mqttServer = "broker.emqx.io"; 
const char* mqttServer = "devbroker.balitower.co.id";
const int mqttPort = 1883;
const char* mqttTopic = "sonar/data";

WiFiClient net;
MQTTClient mqttClient;

Servo myServo;
const int trigPin = 17;
const int echoPin = 16;

void connectToWiFi() {
  Serial.print("Connecting to WiFi");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }
  Serial.println("\nConnected to WiFi");
  Serial.print("IP Address: ");
  Serial.println(WiFi.localIP());
}

void connectToMQTT() {
  while (!mqttClient.connected()) {
    Serial.print("Connecting to MQTT...");
    String clientId = "ESP32Client-" + String(random(0xffff), HEX); // Gunakan Client ID unik
    if (mqttClient.connect(clientId.c_str())) {  
      Serial.println("Connected to MQTT");
      mqttClient.subscribe(mqttTopic); // Opsional: Subscribe untuk menerima data
    } else {
      Serial.print("Failed to connect, rc=");
      Serial.print(mqttClient.lastError());
      Serial.println(" trying again in 5 seconds");
      delay(5000);
    }
  }
}

void publishData(int angle, long distance) {
  String payload = String(angle) + "," + String(distance);
  bool success = mqttClient.publish(mqttTopic, payload);
  // if (success) {
  //   Serial.println("Published: " + payload); 
  // } else {
  //   Serial.println("Failed to publish: " + payload);
  // }
}

long measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000); // Timeout 30ms untuk mencegah hang
  if (duration == 0) {
    Serial.println("No echo detected!");
    return -1; // Indikasi tidak ada jarak terukur
  }
  return duration * 0.034 / 2;  
}

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

  // Inisialisasi WiFi
  connectToWiFi();

  // Inisialisasi MQTT
  mqttClient.begin(mqttServer, mqttPort, net);

  // Inisialisasi Servo
  myServo.attach(26);

  // Inisialisasi Sensor Ultrasonik
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.println("Setup completed");
}

void loop() {
  if (!mqttClient.connected()) {
    connectToMQTT();
  }
  mqttClient.loop();

  // Pindahkan servo dari 0° hingga 180° dan kirim data
  for (int angle = 0; angle <= 180; angle += 2) {
    myServo.write(angle);
    delay(100); // Kurangi delay agar loop lebih responsif
    long distance = measureDistance();
    if (distance != -1) { // Hanya publish jika jarak valid
      publishData(angle, distance);
    }
  }

  // Pindahkan servo kembali dari 180° hingga 0° dan kirim data
  for (int angle = 180; angle >= 0; angle -= 2) {
    myServo.write(angle);
    delay(100); 
    long distance = measureDistance();
    if (distance != -1) {
      publishData(angle, distance);
    }
  }
}