#include <Servo.h> 
Servo top_left_eyelid; //creates top left eyelid servo object
Servo bottom_left_eyelid; //creates bottom left eyelid servo object
Servo top_right_eyelid; //creates top right eyelid servo object
Servo bottom_right_eyelid; //creates bottom right eyelid servo object
Servo myservo; //creates servo object
const int echopin1 = 2 ; //sets echopin1 @pin 2
const int trigpin1 = 4; //sets trigpin1 @pin 4
const int echopin2 = 12; //sets echopin2 @pin 12
const int trigpin2 = 13; //sets trigpin2 @pin 13
const int servo = 3; //sets servo @pin 3
long Rightduration, Leftduration, Rightinch, Leftinch; //establishes the variables of the duration and sets distance in inches
int threshold = 25; //Sets the sensor threshold at 12 inches
int angle = 120; //Sets the Initial angle
int rot = 5; //Sets the angle of rotation change
int minRange = 0; //sets the minimum angle of rotation
int maxRange = 160; //sets the maximum angle of rotation
unsigned long previousMillis = 0; //will store the last time eyes blinked
const long interval = 500; //interval at which the eyes will blink
int counter = 0; //sets the counter to 0

void setup() {
  top_left_eyelid.attach(5); //attached the servo on pin 10
  bottom_left_eyelid.attach(10); //attached the servo on pin 11
  top_right_eyelid.attach(6); //attached the servo on pin 5
  bottom_right_eyelid.attach(11); //attached the servo on pin 6
  myservo.attach(9); //attaches the servo on pin 3
  pinMode(trigpin1, OUTPUT); //trigpin1 is set as output
  pinMode(trigpin2, OUTPUT); //trigpin2 is set as output 
  pinMode(echopin1, INPUT); //echopin1 is set as input
  pinMode(echopin2, INPUT); //echopin2 is set as input
  Serial.begin(9600); // 5Starts the serial communication
}

void loop() {
  digitalWrite(trigpin1, LOW); //sets the trigpin1 to give low pulse
  delayMicroseconds(3); //duration is 3 microseconds
  digitalWrite(trigpin1, HIGH); //sets the trigpin1 to give high pulse
  delayMicroseconds(5); //duration is 5 microseconds
  digitalWrite(trigpin1, LOW); //sets the trigpin1 to give low pulse
  Rightduration = pulseIn(echopin1, HIGH); //reads high pulse

  digitalWrite(trigpin2, LOW); //sets the trigpin2 to give low pulse
  delayMicroseconds(3); //duration is 3 microseconds
  digitalWrite(trigpin2, HIGH); //sets the trigpin2 to give high pulse
  delayMicroseconds(5); //duration is 5 microseconds
  digitalWrite(trigpin2, LOW); //sets the trigpin2 to give low pulse
  Leftduration = pulseIn(echopin2, HIGH); //reads high pulse

  //this will convert the elapsed time into the distance
  Rightinch = microsecondsToInches(Rightduration);  
  Leftinch = microsecondsToInches(Leftduration);

  follow(); //follows the movement
  Serial.print("Right Distance: "); //prints right distance to serial monitor
  Serial.print(Rightinch); //prints right distance to serial monitor
  Serial.print("; Left Distance: "); //prints left distance to serial monitor
  Serial.println(Leftinch); //prints left distance to serial monitor
  
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    // save the last time you blinked the LED
    previousMillis = currentMillis;
    counter = counter + 1;
    if (counter % random(5,15) == 0) {
      close_eye();
    } else {
      open_eye();
    }
  }
}

long microsecondsToInches(long microseconds)

{ 
// The speed of sound is 340 m/s or 73.746 microseconds per inch.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance traveled.
  return microseconds / 74 / 2;
}

void follow() { //conditions for the follow command
  if (Leftinch <= threshold || Rightinch <= threshold)
  {
    if (Leftinch + rot < Rightinch) 
    {
      angle = angle - rot;
    }
    if (Rightinch + rot < Leftinch)
    {
      angle = angle + rot;
    }
  }
  if (angle > maxRange)
  {
    angle = maxRange;
  }
  if (angle < minRange)
  {
    angle = minRange;
  }
  myservo.write(angle);
}

void open_eye() {
  top_left_eyelid.write(55);
  bottom_left_eyelid.write(50);
  top_right_eyelid.write(2);
  bottom_right_eyelid.write(120);
}

void close_eye() {
  top_left_eyelid.write(2);
  bottom_left_eyelid.write(125);
  top_right_eyelid.write(46);
  bottom_right_eyelid.write(55);
}