#include <Arduino.h>
#include <Servo.h>

// ─── Pinos (Nucleo C031C6) ────────────────────────────────────────────────────
#define TRIG_PIN    D2   // Ultrassônico TRIG
#define ECHO_PIN    D3   // Ultrassônico ECHO
#define SERVO_PIN   D4   // Sinal do Servomotor
#define LED_PIN     D5   // LED de alerta
#define BUZZER_PIN  D6   // Buzzer

// ─── Parâmetros do Radar ──────────────────────────────────────────────────────
#define DIST_ALERTA    50   // cm — abaixo disso acende LED e toca buzzer
#define DIST_MIN        5   // cm — distância mínima considerada
#define SERVO_STEP      1   // graus por iteração
#define SERVO_DELAY    15   // ms entre cada passo do servo

// ─── Parâmetros do Buzzer ─────────────────────────────────────────────────────
#define FREQ_LONGE    500   // Hz — objeto na borda do alerta (50 cm)
#define FREQ_PERTO   3000   // Hz — objeto muito próximo (5 cm)
#define BEEP_MS       80   // duração de cada bip (ms)

// ─── Globals ──────────────────────────────────────────────────────────────────
Servo radar;
int  angulo  = 0;
bool subindo = true;

// ─── Função: mede distância em cm ─────────────────────────────────────────────
float medirDistancia() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duracao = pulseIn(ECHO_PIN, HIGH, 30000);
  if (duracao == 0) return 999.0;
  return duracao * 0.0343f / 2.0f;
}

// ─── Função: frequência proporcional à proximidade ────────────────────────────
int calcularFreq(float dist) {
  if (dist >= DIST_ALERTA) return FREQ_LONGE;
  if (dist <= DIST_MIN)    return FREQ_PERTO;
  float t = (DIST_ALERTA - dist) / (float)(DIST_ALERTA - DIST_MIN);
  return (int)(FREQ_LONGE + t * (FREQ_PERTO - FREQ_LONGE));
}

// ─── Setup ────────────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(9600);

  pinMode(TRIG_PIN,   OUTPUT);
  pinMode(ECHO_PIN,   INPUT);
  pinMode(LED_PIN,    OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  radar.attach(SERVO_PIN);
  radar.write(0);
  delay(500);

  Serial.println("=== Radar de Distância — Nucleo C031C6 ===");
}

// ─── Loop ─────────────────────────────────────────────────────────────────────
void loop() {
  radar.write(angulo);

  float dist = medirDistancia();

  Serial.print("Angulo: ");
  Serial.print(angulo);
  Serial.print(" graus | Distancia: ");
  if (dist >= 999.0) {
    Serial.println("> 500 cm (sem objeto)");
  } else {
    Serial.print(dist, 1);
    Serial.println(" cm");
  }

if (dist < DIST_ALERTA) {
    digitalWrite(LED_PIN, LOW);   // perto → acende
    int freq = calcularFreq(dist);
    tone(BUZZER_PIN, freq, BEEP_MS);
  } else {
    digitalWrite(LED_PIN, HIGH);  // longe → apaga
    noTone(BUZZER_PIN);
  }

  if (subindo) {
    angulo += SERVO_STEP;
    if (angulo >= 180) { angulo = 180; subindo = false; }
  } else {
    angulo -= SERVO_STEP;
    if (angulo <= 0)   { angulo = 0;   subindo = true;  }
  }

  delay(SERVO_DELAY);
}