#include <MQTT.h>
#include <WiFi.h>
#include <NusabotSimpleTimer.h>
#include <NewPing.h>

WiFiClient net;
MQTTClient client;
NusabotSimpleTimer timer;

#define TRIGGER_PIN 26
#define ECHO_PIN    27
#define MAX_DISTANCE 400

NewPing cm(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

const char ssid[] = "Wokwi-GUEST";
const char pass[] = "";
int lastDistance = 0;
char messageBuffer[10]; 

void setup() {
  Serial.begin(9600);
  WiFi.begin(ssid, pass);

  // Menunggu hingga WiFi terhubung sebelum memulai
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
  }

  client.begin("shiftrprivate.cloud.shiftr.io", net);
  connect();

  timer.setInterval(1000, []() {
    int distance = cm.ping_cm();

    if (distance != lastDistance) {
      lastDistance = distance;

      snprintf(messageBuffer, sizeof(messageBuffer), "%d", distance);
      publish(messageBuffer);
    }
  });
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    connect();
  }

  if (!client.connected()) {
    connect();
  }

  client.loop();
  timer.run();
  delay(10);
}

void connect() {
  Serial.print("Menghubungkan ke WiFi ");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(100);
  }
  Serial.println(" Terhubung ke WiFi");

  client.setWill("system/otomatic/tol", "Belum terhubung", true, 1);
  while (!client.connect("unix_id", "shiftrprivate", "CeV8a8MdMyDYNV5o")) {
    Serial.print(".");
    delay(100);
  }
  Serial.println("Terhubung ke broker MQTT");
}

void publish(const char* text) {
  client.publish("system/otomatic/tol", text, true, 1);
}