#include "SD.h"                // SD library for SD cards
#define SD_ChipSelectPin 10     // select SD chip select pin 10
#include "TMRpcm.h"            // library to play audio from SD card by Arduino
#include "SPI.h"               // Serial Peripheral Interface Communication
#include "ServoTimer2.h"       // Servo control library

int trigPin = 7;
int echoPin = 6;
long distance;
long duration;
const int detectionRange = 100;     // Distance threshold in cm (1 meter)
unsigned long startTime;            // Variable to store the start time
const char* soundFile = "Snk1.wav"; // Sound file name
int pos = 750;                      // Initial position of servo

TMRpcm tmrpcm;                      // Name for TMRpcm object
ServoTimer2 servo1;                 // Name for Servo object

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  servo1.attach(3);
  servo1.write(pos);               // Set servo to initial position using the pos variable

  tmrpcm.speakerPin = 9;
  tmrpcm.setVolume(5.5);
  Serial.begin(9600);              // Initialize serial communication

  if (!SD.begin(SD_ChipSelectPin)) {  // Initialize SD card
    Serial.println("SD initialization failed!");
    return;
  }
  Serial.println("System initialized. Waiting for object detection...");
}

void loop() {
  ultra();  // Measure the distance

  if (distance <= detectionRange) {  // If an object is detected within detectionRange
    //delay(3000); // Optional delay before activation
    
    Serial.println("Object detected within range. Activating servo and sound.");
    tmrpcm.play(soundFile);  // Play the sound file only after detection
    startTime = millis();    // Record the start time

    // Run the servo and sound for 6 seconds
    while (millis() - startTime < 1000) {  // Adjusted to 6 seconds (6000 ms)
      servo1.write(1600);  // Move the servo to position 1600
      Serial.println("Moving servo and playing sound.");
      delay(500);          // Short delay to hold position
    }

    // After 6 seconds, return the servo to the initial position and stop sound
    servo1.write(pos);      // Return servo to initial position using the pos variable
    tmrpcm.stopPlayback();  // Stop the audio playback
    Serial.println("Servo returned to initial position and sound stopped.");

    // Wait until the object moves away
    while (distance < detectionRange) {
      ultra();  // Continuously update the distance until the object moves away
      delay(500);
    }
    Serial.println("Object has moved away. Ready for next detection.");
  } else {
    Serial.println("No object detected within range.");
  }

  delay(1000);  // Delay before the next loop iteration
}

void ultra() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  Serial.print("Measured distance: ");
  Serial.print(distance);
  Serial.println(" cm");
}