#include <Servo.h>

Servo servo1;
Servo servo2;

const int trigPin = 11;
const int echoPin = 12;
const int greenLed = 3;

long duration;
int distance;
int sum = 0;
int count = 0;
int numOfLoops = 300;
int maxDistance = 60; // for 60 cm windows
int servoVal;

void setup() {
  servo1.attach(9);
  servo2.attach(10);

  pinMode(greenLed, OUTPUT);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
}

void loop() {
  while (true) {
    /* detecting object and calculating the distance */
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, HIGH);

    duration = pulseIn(echoPin, HIGH);
    distance = duration * 0.034 / 2;
    distance = distance + (distance / 15);

    if (distance <= maxDistance) {
      digitalWrite(greenLed, HIGH); // turn on laser
      servo1.write(90); // stop servo movement
      servo2.write(90);
    } else {
      digitalWrite(greenLed, LOW); // turn off laser
      break; // exit loop
    }
  }

  /* calculating the average of the distance and printing the results */
  if (count < numOfLoops) {
    if (distance < 200 && distance > 0) {
      sum += distance;
      count++;
    }
  } else {
    if ((sum / numOfLoops) < maxDistance) { //checking if a new object is arrived
      Serial.println(sum / numOfLoops);
      startExpelling();
    }
    count = 0;
    sum = 0;
  }
}

void startExpelling() {
  int j = 170;

  //up and down movment
  while (j > 70) {
    servo2.write(j);
    delay(100);
    j--;
  }
  moveLeftToright();
  moveLeftToright();
  moveLeftToright();

  delay(100);
}

//left to right movment
void moveLeftToright() {
  int i = 90;

  while (i < 100) {
    servo1.write(i);
    delay(100);
    i++;
  }
  while (i > 80) {
    servo1.write(i);
    delay(100);
    i--;
  }


  while (i < 90) {
    servo1.write(i);
    delay(100);
    i++;
  }
}