#include <Servo.h>  //Include servo library
#include <IRremote.h>  //Include IR library

#define trigPin 13 //Define pin 13 as trig pin
#define echoPin 12 //Define pin 12 as echo pin
#define led 7  //Define pin 7 as red led+buzzer pin
#define led2 8 // Define pin 8 as green pin

Servo Middleservo; //Define first servo
Servo Rightservo; //Define second servo
Servo Leftservo; //Define third servo

int IR_Recv = 3; //IR Receiver Pin 3
int shutdown = 0;

IRrecv irrecv(IR_Recv); //Create the receiver object
decode_results results; //Decode the results from the sensor

void setup()
{
  Serial.begin (9600); //Start the serial communication at 9600 bits per second

  pinMode(trigPin, OUTPUT); // make the trigger's pin an output:
  pinMode(echoPin, INPUT); // make the echo's pin an input:
  pinMode(led, OUTPUT); // make the red led+buzzer pin an output:
  pinMode(led2, OUTPUT); // make the green led pin an output:

  Middleservo.attach(9); //Attach the Middleservo variable to pin 9
  Rightservo.attach(10); //Attach the Rightservo variable to pin 10
  Leftservo.attach(11); //Attach the Leftservo variable to pin 11

    irrecv.enableIRIn(); // Starts the receiver
  irrecv.blink13(true); //Enable blinking the LED when during reception
}

void loop()
{
  if (irrecv.decode(&results)) //Attempt to receive a IR code
  {
    if(results.value == 16753245)
    {
      //Set the variables as 0
      long duration = 0, distance = 0;
      // Start Ranging
      digitalWrite(trigPin, LOW);
      delayMicroseconds(2);
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);
      // Compute distance
      duration = pulseIn(echoPin, HIGH);
      distance = (duration/2) / 29.1;

      if(distance <= 20) //If the distance is lower or equal to 20 CM do below
      {
        digitalWrite(led,HIGH); // sets the red led+buzzer on
        digitalWrite(led2,LOW); // sets the green led off

        Middleservo.write(75); // set Middleservo to 75 degrees
        delay(200); // waits for a 200 milliseconds
        Leftservo.write(110); // set Leftservo to 110 degrees
        delay(50); // waits for a 50 milliseconds
        Rightservo.write(80); // set Rightservo to 80 degrees
        delay(50); // waits for a 50 milliseconds
        Middleservo.write(95); // set Middleservo to 95 degrees
        delay(200); // waits for a 200 milliseconds
        Rightservo.write(105); // set Rightservo to 105 degrees
        delay(50); // waits for a 50 milliseconds
        Leftservo.write(80); // set Leftservo to 80 degrees
        delay(50); // waits for a 50 milliseconds
      }
      else //If the distance isnt lower or equal to 20 CM do below:
      {
        digitalWrite(led,LOW); // sets the red led+buzzer off
        digitalWrite(led2,HIGH); // sets the green led on

        Middleservo.write(65); // set Middleservo to 65 degrees
        delay(200); // waits for a 200 milliseconds
        Leftservo.write(80);  // set Leftservo to 80 degrees
        delay(50); // waits for a 50 milliseconds
        Rightservo.write(80);  // set Rightservo to 80 degrees
        delay(50); // waits for a 50 milliseconds
        Middleservo.write(105); // set Middleservo to 105 degrees
        delay(200); // waits for a 200 milliseconds
        Rightservo.write(110);  // set Rightservo to 110 degrees
        delay(50); // waits for a 50 milliseconds
        Leftservo.write(110);  // set Leftservo to 110 degrees
        delay(50); // waits for a 50 milliseconds
      }
      if (distance >= 200 || distance <= 0)
      { //If the distance is bigger or equal to 200 and lower and equal to 0
        Serial.println("Out of range"); // print "Out of range"
      }
      else
      {  //if not
        Serial.print(distance); //print distance
        Serial.println(" cm"); //print cm below the distance
      }
    }
    else //if the result isnt 16753245 do below
    {
      irrecv.resume(); //reset the receiver and prepare it to receive another code
    }
  }
}