#include <ESP32Servo.h>
//#include <ESP32_Button.h>
#include <ezButton.h>
#include <Button.h>


#define TRIG_PIN  23  // ESP32 pin GPIO23 connected to Ultrasonic Sensor's TRIG pin
#define ECHO_PIN  22  // ESP32 pin GPIO22 connected to Ultrasonic Sensor's ECHO pin
#define BUTTON_PIN  18 // ESP32 pin GPIO18, which connected to button
#define SERVO_PIN 17  // ESP32 pin GPIO26 connected to Servo Motor's pin
#define LED_PIN     21 // ESP32 pin GPIO21, which connected to led
#define DISTANCE_THRESHOLD  10 // centimeters


Servo servo;
ezButton button(BUTTON_PIN);  // create ezButton object that attach to pin 7;

// variables will change:
float duration_us, distance_cm, duration_us1, distance_cm1;
int led_state = LOW;   // the current state of LED

void setup() {
  Serial.begin(9600);         // initialize serial
  pinMode(TRIG_PIN, OUTPUT); // set ESP32 pin to output mode
  pinMode(ECHO_PIN, INPUT);  // set ESP32 pin to input mode
  pinMode(LED_PIN, OUTPUT);   // set ESP32 pin to output mode
  button.setDebounceTime(50); // set debounce time to 50 milliseconds
  servo.attach(SERVO_PIN);   // attaches the servo on pin 9 to the servo object
  servo.write(0);
}

void loop() {
  button.loop(); // MUST call the loop() function first
  digitalWrite(TRIG_PIN, HIGH);
  //digitalWrite(TRIG_PIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  //digitalWrite(TRIG_PIN1, LOW);
  if (distance_cm < DISTANCE_THRESHOLD){
    servo.write(90);
    //servo1.write(90); // rotate servo motor to 90 degree
  } else {
      if (button.isPressed()) {
      Serial.println("The button is pressed");

      // toggle state of LED
      if(led_state == LOW){
        led_state = HIGH;
        servo.write(90);
      }else{
        led_state = LOW;
        servo.write(0);
      }
    

      // control LED arccoding to the toggleed sate
      digitalWrite(LED_PIN, led_state);
    }
  }
}