#include <Servo.h>
Servo servo_11;

#define green 3
#define blue 4
#define red 5
#define servo 11
#define alarm 6
int button = 2;
int switch_1 = 8;
long duration;
int distance; 
const int trigPin = 9;
const int echoPin = 10;

int pressed;
int activated;

void setup()
{
  
  pinMode (trigPin, OUTPUT);
  pinMode (echoPin, INPUT);
  pinMode (button, INPUT);
  pinMode(servo, OUTPUT);
  pinMode(alarm, OUTPUT);
  pinMode(red, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(blue, OUTPUT);
   
  servo_11.attach(11, 500, 2500);

  Serial.begin(9600);

 Serial.println();
 Serial.print("--begin test-- ");
 digitalWrite(red, HIGH);
 digitalWrite(green, HIGH);
 digitalWrite(blue, HIGH);
 analogWrite (alarm, 120);
 servo_11.write(80);
 delay(800);
 servo_11.write(0);

 digitalWrite(red, LOW);
 digitalWrite(green, LOW);
 digitalWrite(blue, LOW);
 analogWrite (alarm, 0);
 Serial.print("--test complete-- ");
 Serial.println();
delay(1000);
} 

void loop()
{
  activated = digitalRead(switch_1);
  if (activated == 1){
  Read_ultrasonic ();
  READ_AND_PRINT_ALL_SENSOR ();
defense_system ();
  }
 

} 






void Read_ultrasonic (){
digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  
  distance = duration * 0.034 / 2;
  
}

void READ_AND_PRINT_ALL_SENSOR () {
    Serial.println();
    Serial.print("   -Distance: ");
    Serial.print(distance);

    pressed = digitalRead(button);
    Serial.print("   -button: ");
    Serial.print(pressed);


    
    Serial.print("   -active: ");
    Serial.print(activated);
   Serial.println();


}

void defense_system () {
if (distance<100){
digitalWrite(blue, LOW);
digitalWrite(red, HIGH);
digitalWrite(green, LOW);
analogWrite (alarm, 120);
Serial.println();
Serial.print("    too close!    ");
Serial.println();
delay(1000);
servo_11.write(0);
}
if (distance>120) {
digitalWrite(blue, HIGH);
digitalWrite(red, LOW);
digitalWrite(green, LOW);
analogWrite (alarm, 0);
Serial.println();
Serial.print("    too far!    ");
Serial.println();
delay(1000);
servo_11.write(0);
}
if (distance<120&& distance>100) {
digitalWrite(green, HIGH);
digitalWrite(blue, LOW);
digitalWrite(red, LOW);
analogWrite (alarm, 0);
Serial.println();
Serial.print("    good distance    ");
Serial.println();
delay(1000);

}
if ( distance<120&& distance>100&&pressed==1) {
Serial.println();
Serial.print("   brushing   ");
Serial.println();
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);
servo_11.write(100);
delay(200);
servo_11.write(120);
delay(200);

}
}