#include <avr/sleep.h>
#define trigPin  9
#define echoPin 8
#define buzzerPin 5
#define proximityLED 7  // used inplace of haptic motor to test it

long previousMillis = 0;
const long interval = 2000; // Interval for alert tones (2 seconds)

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(proximityLED, OUTPUT); // Set the proximity LED pin as an output
}

void loop() {
  long duration, distance;

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.0343 / 2;

  // Print distance to Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance < 40 && distance >= 20) {
    // No LED indication for medium proximity
    digitalWrite(proximityLED, LOW);
  }

  if (distance < 20 && distance >= 10) {
    // LED indication for closer proximity
    digitalWrite(proximityLED, HIGH);
    tone(buzzerPin, 1000, 200); // Beep for 200ms
  } else {
    digitalWrite(proximityLED, LOW);
    noTone(buzzerPin);
  }

  if (distance < 10) {
    // LED indication for very close proximity
    digitalWrite(proximityLED, HIGH);
    tone(buzzerPin, 1000, 800); // Beep for 800ms
  } else {
    noTone(buzzerPin);
  }

  delay(100); // Add a small delay for stability

  // Check if it's time to enter sleep mode
  if (millis() - previousMillis >= interval) {
    previousMillis = millis();
    sleep();
  }
}

void sleep() {
  // Turn off the LED and buzzer before sleeping
  digitalWrite(proximityLED, LOW);
  digitalWrite(buzzerPin, LOW);
  delay(1); // Allow pins to settle

  // Enter sleep mode
  set_sleep_mode(SLEEP_MODE_PWR_DOWN);
  sleep_enable();
  sleep_mode();
  
  // After waking up, re-enable pins
  digitalWrite(buzzerPin, HIGH);
  sleep_disable();
}