#include <Servo.h>

const int TRIG_PIN = 2;
const int ECHO_PIN = 3;
const int LED_PIN = 4;
const int BUZZER_PIN = 5;
const int SERVO_PIN = 9;

Servo radarServo;

long medirDistancia() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duracao = pulseIn(ECHO_PIN, HIGH);
  long distancia = duracao * 0.034 / 2;

  return distancia;
}

void controlarAlarme(long distancia) {
  if (distancia > 0 && distancia < 50) {
    digitalWrite(LED_PIN, HIGH);

    int frequencia = map(distancia, 50, 0, 500, 3000);
    tone(BUZZER_PIN, frequencia);
  } else {
    digitalWrite(LED_PIN, LOW);
    noTone(BUZZER_PIN);
  }
}

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  radarServo.attach(SERVO_PIN);

  Serial.begin(115200);
}

void loop() {
  for (int angulo = 0; angulo <= 180; angulo++) {
    radarServo.write(angulo);

    long distancia = medirDistancia();

    Serial.print("Angulo: ");
    Serial.print(angulo);
    Serial.print(" | Distancia: ");
    Serial.print(distancia);
    Serial.println(" cm");

    controlarAlarme(distancia);

    delay(20);
  }

  for (int angulo = 180; angulo >= 0; angulo--) {
    radarServo.write(angulo);

    long distancia = medirDistancia();

    Serial.print("Angulo: ");
    Serial.print(angulo);
    Serial.print(" | Distancia: ");
    Serial.print(distancia);
    Serial.println(" cm");

    controlarAlarme(distancia);

    delay(20);
  }
}