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

Servo myservo;
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);

//Servo
const int servoPin = 22;

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

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

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

  client.begin("system-smart.cloud.shiftr.io", net);
  connect();

}

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

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

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

    if (distance != lastDistance) {
      lastDistance = distance;
      if (distance < 75) {
        myservo.write(0);
        snprintf(messageBuffer, sizeof(messageBuffer), "Kucing terdeteksi! Tempat makanan di buka!");
        publish(messageBuffer);
      } else {
        myservo.write(90);
        snprintf(messageBuffer, sizeof(messageBuffer), "Kucing tidak terdeteksi! Tempat makanan di tutup!");
        publish(messageBuffer);
      }
    }
  });

  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/pet/feeder/fadhil", "Belum terhubung", true, 1);
  while (!client.connect("unix_id", "system-smart", "fadhil")) {
    Serial.print(".");
    delay(100);
  }
  Serial.println("Terhubung ke broker MQTT");
}

void publish(const char* text) {
  client.publish("system/pet/feeder/fadhil", text, true, 1);
}