#include <ServoTimer2.h>
#include <TMRpcm.h>
#include <SPI.h>

ServoTimer2 servo1;
TMRpcm audio;

const int trigPin = 7;  // Pin for the trigger of ultrasonic sensor
const int echoPin = 6;  // Pin for the echo of ultrasonic sensor
const int soundPin = 9; // Pin for the speaker
int pos = 750;          // Initial position of the servo
long duration;
int distance;
bool objectDetected = false;  // Detection flag

void setup() {
  Serial.begin(9600);   // Initialize Serial communication at 9600 baud rate
  servo1.attach(4);    // Attach servo on pin 10
  servo1.write(pos);    // Set initial position to 750

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  audio.speakerPin = soundPin; // Attach speaker to sound pin
  audio.setVolume(6);          // Set volume level (0-7)
}

void loop() {
  // Send a 10us pulse to trigger the ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echo pin and calculate distance
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2; // Convert to distance in cm

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance < 30 && !objectDetected) { // If distance is less than 1 foot and object not yet detected
    objectDetected = true;   // Set detection flag to true
    Serial.println("Object detected! Moving servo and playing sound...");
  
    audio.play("sound.wav"); // Play the wav file (file should be on SD card)
    servo1.write(1600);      // Move servo to 1600
    delay(500);
    servo1.write(1100);
    delay(500);
    servo1.write(1600);
    delay(500);
    servo1.write(1100);
    delay(500);
    servo1.write(1600);
    delay(500);
    servo1.write(1100);
    delay(500);
    servo1.write(1600);
    delay(500);
   

    //delay(3000); // Wait for 3 seconds

    servo1.write(750);       // Return servo to initial position 
    audio.stopPlayback();    // Stop the audio
    Serial.println("Servo returned to original position, sound stopped.");

    // Wait until the object is removed (distance > 1 foot)
    while (distance < 30) {
      // Recalculate the distance
      digitalWrite(trigPin, LOW);
      delayMicroseconds(2);
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigPin, LOW);

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

      Serial.print("Waiting for object to leave... Distance: ");
      Serial.print(distance);
      Serial.println(" cm");

      // Do nothing while the object is still within 1 foot
    }

    objectDetected = false; // Reset detection flag when object leaves
    Serial.println("Object has left. Ready for new detection.");
  }

  // The loop continues to check for the object after it leaves
}