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

#define TRIG_PIN 5
#define ECHO_PIN 18
#define BUZZER_PIN 19
#define SERVO_PIN 13

const char* ssid = "Wokwi-GUEST";
const char* password = "";

const char* mqtt_server = "demo.thingsboard.io";
const char* token = "yzj2kutyfly0hq5m5oip";

WiFiClient espClient;
PubSubClient client(espClient);

Servo myservo;

float readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0) return 999;

  return duration * 0.034 / 2;
}

void connectWiFi() {
  WiFi.begin(ssid, password);
  Serial.print("Connecting WiFi");

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

  Serial.println("\nWiFi connected");
}

void reconnect() {
  while (!client.connected()) {
    Serial.println("Connecting to ThingsBoard...");

    if (client.connect("ESP32", token, "")) {  // 🔥 WAJIB pakai ""
      Serial.println("Connected to ThingsBoard");
    } else {
      Serial.print("Failed, rc=");
      Serial.println(client.state());
      delay(2000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  Serial.print("Token yang dipakai: ");
  Serial.println(token);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  myservo.attach(SERVO_PIN);

  connectWiFi();

  client.setServer(mqtt_server, 1883);
}

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

  float distance = readDistanceCM();
  String statusPalang;

  if (distance <= 200) {
    myservo.write(90);
    tone(BUZZER_PIN, 1000);

    statusPalang = "TUTUP";
  } else {
    myservo.write(0);
    noTone(BUZZER_PIN);

    statusPalang = "BUKA";
  }

  String payload = "{";
  payload += "\"distance\":" + String(distance) + ",";
  payload += "\"palang\":\"" + statusPalang + "\"";
  payload += "}";

  if (client.publish("v1/devices/me/telemetry", payload.c_str())) {
    Serial.println("Data sent:");
    Serial.println(payload);
  } else {
    Serial.println("Failed to send data");
  }

  delay(2000);
}