#include <NewPing.h>
#include <NewTone.h>

#define DEBUG true // false when release
#define DEBUG_PRINT(x) do { if (DEBUG) Serial.print(x); } while (0)
#define DEBUG_PRINTLN(x) do { if (DEBUG) Serial.println(x); } while (0)

#define TRIGGER_PIN 12
#define ECHO_PIN 11
#define BUZZER_PIN 9
#define REVERSE_TRIGGER_PIN 2 // Pin connected to reverse light (via 5V regulator)
#define MAX_DISTANCE 200 // Maximum distance to detect (in cm)

// Buzzer timing variables
unsigned long previousMillis = 0;
int buzzerState = 0; // 0: Off, 1: Short Beep, 2: Long Beep

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {
  Serial.begin(115200); // Debugging output
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(REVERSE_TRIGGER_PIN, INPUT); // Monitor reverse light line
}

void loop() {
  // Check if reverse light is active
  if (digitalRead(REVERSE_TRIGGER_PIN) == HIGH) {
    delay(50); // Sensor polling interval
    int distance = sonar.ping_cm();

    // Debugging output
    DEBUG_PRINT("Distance: ");
    DEBUG_PRINT(distance);
    DEBUG_PRINTLN(" cm");

    // Handle distance-to-sound logic
    handleBuzzer(distance);
  } else {
    // Reverse light off, ensure buzzer is silent
    noNewTone(BUZZER_PIN);
  }
}

void handleBuzzer(int distance) {
  unsigned long currentMillis = millis();

  if (distance > 0 && distance <= 10) {
    // Critical range, continuous tone
    NewTone(BUZZER_PIN, 2000);
    buzzerState = 0; // Reset any previous state
  } else if (distance > 10 && distance <= 50) {
    // Mid-range, intermittent beeps
    if (currentMillis - previousMillis >= 200) {
      previousMillis = currentMillis;
      if (buzzerState == 0) {
        NewTone(BUZZER_PIN, 1000);
        buzzerState = 1;
      } else {
        noNewTone(BUZZER_PIN);
        buzzerState = 0;
      }
    }
  } else if (distance > 50 && distance < MAX_DISTANCE) {
    // Far range, slower beeps
    if (currentMillis - previousMillis >= 500) {
      previousMillis = currentMillis;
      if (buzzerState == 0) {
        NewTone(BUZZER_PIN, 500);
        buzzerState = 2;
      } else {
        noNewTone(BUZZER_PIN);
        buzzerState = 0;
      }
    }
  } else {
    // Out of range or invalid detection, no sound
    noNewTone(BUZZER_PIN);
    buzzerState = 0;
  }
}