#include <PubSubClient.h>
#include <WiFi.h>
#include <ESP32Servo.h>
const char* mqttServer    = "armadillo.rmq.cloudamqp.com";
const int mqttPort        = 1883;
const char* mqttUser      = "zqaoagkp:zqaoagkp";
const char* mqttPassword  = "6Gab186xZZpI82VNUDjXLrYeoSi9ylRd";

WiFiClient espClient;
PubSubClient client(espClient);

// Pin yang digunakan untuk sensor pertama HC-SR04
const int trigPin1 = 12;
const int echoPin1 = 13;

// Pin yang digunakan untuk sensor kedua HC-SR04
const int trigPin2 = 14;
const int echoPin2 = 15;

// Pin yang digunakan untuk servo motor
const int servoPin = 4;


// Objek Servo
Servo servo;

void setup() {
  Serial.begin(9600);
WiFi.begin("Wokwi-GUEST", "", 6);
 
  while (WiFi.status() != WL_CONNECTED) 
  {
    delay(500);
    Serial.println("Connecting to WiFi..");
  }
  Serial.println("Connected to the WiFi network");
 
  client.setServer(mqttServer, mqttPort);
  //client.setCallback(callback);
 
  while (!client.connected()) 
  {
    Serial.println("Connecting to MQTT...");
 
    if (client.connect("ESP8266Client", mqttUser, mqttPassword )) 
    {
      Serial.println("connected");  
    } else
     {
      Serial.print("failed with state ");
      Serial.print(client.state());
      delay(2000);
    }
  stattopic = client.subscribe("kapasitas");


  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);

  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  servo.attach(servoPin);
}

void loop() {
  // Mengukur jarak dari sensor pertama
  long duration1 = measureDistance(trigPin1, echoPin1);

  // Mengukur jarak dari sensor kedua
  long duration2 = measureDistance(trigPin2, echoPin2);

  // Menghitung jarak berdasarkan waktu pantulan
  float distance1 = duration1 * 0.034 / 2;
  float distance2 = duration2 * 0.034 / 2;

  // Menampilkan jarak ke Serial Monitor
  Serial.print("Jarak Sensor 1: ");
  Serial.print(distance1);
  Serial.println(" cm");

  Serial.print("kapasitas: ");
  Serial.print(distance2);
  Serial.println(" cm");

  // Jika jarak dari sensor pertama adalah di bawah 20 cm, menggerakkan servo
  if (distance1 < 20 && distance2 >5) {
    servo.write(180); // Menggerakkan servo ke posisi tengah
    delay( 5000);
    servo.write(0); // Menggerakkan servo ke posisi awal
    delay(1000);
  }
//  // Jika jarak dari sensor kedua adalah di bawah 10 cm, servo tidak bergerak
//  if else (distance2 < 10) {
//    servo.write(0); // Menggerakkan servo ke posisi awal
//    delay(1000);
//  }

  delay(1000);
}


long measureDistance(int trigPin, int echoPin) {
  // Mengirimkan pulsa ultrasonik
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Menerima waktu pantulan gelombang ultrasonik
  long duration = pulseIn(echoPin, HIGH);

  return duration;

   Serial.println(stattopic);
  
  client.publish("kapasitas", String(distance2).c_str());
 
  delay(1000);
  client.loop(); 
}