#include <Servo.h>
Servo myservo;

const int Echo1 = 4;
const int Echo2 = 7;
const int Echo3 = 0;
const int Trig = 2;
const int R1 = 13;
const int G1 = 12;
const int R2 = 11;
const int Y2 = 3;
const int G2 = 9;
const int Buzzer = 1;
const int Servo = 10;
int Melody = 500;
const int SW1 = 6;
const int SW2 = 5;
int SW1Val = 0;
int SW2Val = 0;
int ServoR = 0;
int ServoP = 0;

// variables will change:
int buttonState = 0;         // variable for reading the pushbutton status
int distance1 = 0;
int distance2 = 0;
int distance3 = 0;

void setup() {
  Serial.begin(115200);
  myservo.attach(Servo);
  pinMode(Echo1, INPUT);
  pinMode(Echo2, INPUT);
  pinMode(Echo3, INPUT);
  pinMode(Trig, OUTPUT);
  pinMode(R1, OUTPUT);
  pinMode(G1, OUTPUT);
  pinMode(R2, OUTPUT);
  pinMode(Y2, OUTPUT);
  pinMode(G2, OUTPUT);
  pinMode(Buzzer, OUTPUT);
  pinMode(SW1, INPUT_PULLUP);
  pinMode(SW2, INPUT_PULLUP);
  myservo.write(180);
  delay(250);
  myservo.write(0);
  delay(500);
  myservo.write(90);
}

void loop() {
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  //buttonState = digitalRead(buttonPin);
  distance1 = pulseIn(Echo1, HIGH) / 58;
  delay(100);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  distance2 = pulseIn(Echo2, HIGH) / 58;
  delay(100);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  distance3 = pulseIn(Echo3, HIGH) / 58;
  Serial.print("Ultrasonic №1:");
  Serial.println(distance1);
  Serial.print("Ultrasonic №2:");
  Serial.println(distance2);
  Serial.print("Ultrasonic №3:");
  Serial.println(distance3);
  Serial.print("ServoR: ");
  Serial.println(ServoR);
  Serial.println(" ");
  SW1Val = digitalRead(SW1);
  SW2Val = digitalRead(SW2);
  if ((distance1 < 51 && distance2 < 51) || (distance2 < 51 && distance3 < 51) || (distance1 < 51 && distance3 < 51)) {
    ServoR += 1;
    ServoP = ServoR * 10;
    myservo.write(ServoP);
    //delay(250);
    if (ServoR == 18){
      yankeeGoHome();
    }
  }
  else {
    if (ServoR > 0){
      ServoR -=1;
      ServoP = ServoR * 10;
      myservo.write(ServoP);
    }  
  }
  digitalWrite(R1, HIGH);
  digitalWrite(G1, LOW);
  digitalWrite(R2, LOW);
  digitalWrite(Y2, LOW);
  digitalWrite(G2, HIGH);
}

  void yankeeGoHome(){
  delay(4000);
  ServoR = 0;
  digitalWrite(G2, LOW);
  delay(500);
  digitalWrite(G2, HIGH);
  delay(500);
  digitalWrite(G2, LOW);
  delay(500);
  digitalWrite(G2, HIGH);
  delay(500);
  digitalWrite(G2, LOW);
  digitalWrite(Y2, HIGH);
  delay(1000);
  digitalWrite(Y2, LOW);
  digitalWrite(R2, HIGH);
  delay(1000);
  digitalWrite(R1, LOW);
  digitalWrite(G1, HIGH);
  for (int x = 1; x<9; x++){
    if (SW1Val == 0) {
      for (int Melody = 500; Melody<2101; Melody +=200){
        tone(Buzzer, Melody);
        Serial.println(Melody);
        delay(50);
      };
      noTone(Buzzer);
      delay(250);
    }
    else {
      delay(500);
    };
    if (SW2Val == 0) {
      for (int Melody = 2100; Melody>499; Melody -= 200){
         tone(Buzzer, Melody);
          Serial.println(Melody);
          delay(50);
        }
        noTone(Buzzer);
        delay(250);
    }
    else {
      delay(500);
    };  
  };
  digitalWrite(G1, LOW);
  delay(500);
  digitalWrite(G1, HIGH);
  delay(500);
  digitalWrite(G1, LOW);
  delay(500);
  digitalWrite(G1, HIGH);
  delay(500);
  digitalWrite(G1, LOW);
  digitalWrite(Y2, HIGH);
  digitalWrite(R1, HIGH);
  delay(1000);
  digitalWrite(R2, LOW);
  digitalWrite(Y2, LOW);
  digitalWrite(G2, HIGH);
  float ServoPF = 180.0;
  for (int t = 0; t < 40; t++){
    ServoPF = ServoPF - 4,5;
    myservo.write(ServoPF);
    delay(500);
  }
}