#include <NewPing.h> // Library untuk Sensor Ultrasonic
#include <Servo.h>   // Library untuk Servo
#include <WiFi.h>    // Library WiFi (untuk ESP8266/ESP32)
#include <UniversalTelegramBot.h> // Library Telegram Bot

#define trigPin 12  // Set Trigger HCSR04 di Pin digital 12
#define echoPin 13  // Set Echo HCSR04 di Pin digital 13
#define MAX_DISTANCE 500 // Set jarak maksimal

NewPing sonar(trigPin, echoPin, MAX_DISTANCE);
Servo myservo; // Buat object 1 buah motor servo

int LEDhijau = 3, LEDmerah = 2; // Set Pin LED hijau dan merah
const char* ssid = "Wokwi-GUEST"; // SSID WiFi Wokwi
const char* password = ""; // Password WiFi Wokwi (kosong)
const char* botToken = "7892409234:AAEEMVHRFwWD2OKvtEe_5rJGcMoyr1mkSds"; // Token Bot Telegram Anda
const char* chatID = "5887673880D"; // ID Chat Telegram Anda

WiFiClient client;
UniversalTelegramBot bot(botToken, client);

void setup() {
  Serial.begin(115200); // Kecepatan komunikasi serial
  pinMode(trigPin, OUTPUT); // Set pin Trigger sebagai output
  pinMode(echoPin, INPUT);  // Set pin Echo sebagai input
  pinMode(LEDmerah, OUTPUT); // Set LED merah sebagai output
  pinMode(LEDhijau, OUTPUT); // Set LED hijau sebagai output
  myservo.attach(9);         // Set servo pada pin PWM 9

  // Sambungkan ke WiFi
  WiFi.begin(ssid, password);
  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }
  Serial.println("\nWiFi connected!");
  Serial.println("IP Address: ");
  Serial.println(WiFi.localIP());

  // Inisialisasi posisi servo
  myservo.write(90);
}

void loop() {
  int duration, jarak;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  jarak = (duration / 2) / 29.1;

  Serial.print(jarak);
  Serial.println(" cm");

  if (jarak <= 150) { // Jarak (cm) dapat disesuaikan
    digitalWrite(LEDhijau, LOW); // LED hijau mati
    digitalWrite(LEDmerah, HIGH); // LED merah hidup
    myservo.write(90); // Posisi servo pada 90 derajat
    delay(450);
    myservo.write(0);  // Posisi servo pada 0 derajat
    delay(450);

    // Kirim notifikasi ke Telegram
    String message = "Jarak terdeteksi: " + String(jarak) + " cm.";
    bot.sendMessage(chatID, message, "");
  } else { // Jika jarak lebih dari yang ditentukan
    digitalWrite(LEDmerah, LOW);  // LED merah mati
    digitalWrite(LEDhijau, HIGH); // LED hijau hidup
    myservo.write(90); // Posisi servo pada 90 derajat
  }

  delay(450); // Delay
}