#include <Servo.h>

Servo myservo;  // create servo object to control a servo

#define echoPin 9 // attach pin D2 Arduino to pin Echo of HC-SR04
#define trigPin 8 //attach pin D3 Arduino to pin Trig of HC-SR04
#define BUSY 1
#define NOT_BUSY 0

// defines variables
long duration; // variable for the duration of sound wave travel
int distance; // variable for the distance measurement
int previousState = 0;
int busyCount = 0;
void setup() {
  myservo.attach(10);  // attaches the servo on pin 9 to the servo object
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT
  Serial.begin(9600); // // Serial Communication is starting with 9600 of baudrate speed
  Serial.println("Ultrasonic Sensor HC-SR04 Test"); // print some text in Serial Monitor
  Serial.println("with Arduino UNO R3");
  myservo.write(40);
  delay(1000);
}
void loop() {
  int value = calculate_distance();
  if(value>30){
    previousState = NOT_BUSY;
    if(busyCount>=30){
      flashToilet();
      }
    busyCount = 0;
    }
  else if(value<=30){
    if(previousState == BUSY){
      busyCount++;
      delay(1000);
      } 
     previousState = BUSY;  
    }
}

void flashToilet(){ 
  myservo.write(140);
  delay(3000);
  myservo.write(40);
  delay(1000);
  }

int calculate_distance(){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
  // Displays the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
  return distance;
}