#include <Wire.h>
#include <MPU6050.h> // Include the MPU6050 library

// Define ultrasonic sensor pins
#define TRIG_PIN_1 18    // ESP32 pin connected to Ultrasonic Sensor 1's TRIG pin
#define ECHO_PIN_1 5     // ESP32 pin connected to Ultrasonic Sensor 1's ECHO pin
#define TRIG_PIN_2 19    // ESP32 pin connected to Ultrasonic Sensor 2's TRIG pin
#define ECHO_PIN_2 23    // ESP32 pin connected to Ultrasonic Sensor 2's ECHO pin
#define TRIG_PIN_3 15    // ESP32 pin connected to Ultrasonic Sensor 3's TRIG pin
#define ECHO_PIN_3 17    // ESP32 pin connected to Ultrasonic Sensor 3's ECHO pin

// Define LED and buzzer pins
#define LED_PIN_1 2      // Pin for LED 1 (representing motor 1)
#define LED_PIN_2 4      // Pin for LED 2 (representing motor 2)
#define LED_PIN_3 16     // Pin for LED 3 (representing motor 3)

#define BUZZER_PIN_1 25   // Pin for Buzzer 1
#define BUZZER_PIN_2 26   // Pin for Buzzer 2
#define BUZZER_PIN_3 27   // Pin for Buzzer 3

// Variables for distance measurement
float duration_us_1, distance_cm_1;
float duration_us_2, distance_cm_2;
float duration_us_3, distance_cm_3;

// MPU6050 object
MPU6050 mpu;

// Variables for fall detection
int16_t ax, ay, az; // Acceleration values

void setup() {
  // Begin serial port
  Serial.begin(9600);

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1); // Loop indefinitely if MPU6050 fails
  }

  // Configure pins for Sensor 1
  pinMode(TRIG_PIN_1, OUTPUT);
  pinMode(ECHO_PIN_1, INPUT);
  pinMode(LED_PIN_1, OUTPUT);
  pinMode(BUZZER_PIN_1, OUTPUT);

  // Configure pins for Sensor 2
  pinMode(TRIG_PIN_2, OUTPUT);
  pinMode(ECHO_PIN_2, INPUT);
  pinMode(LED_PIN_2, OUTPUT);
  pinMode(BUZZER_PIN_2, OUTPUT);

  // Configure pins for Sensor 3
  pinMode(TRIG_PIN_3, OUTPUT);
  pinMode(ECHO_PIN_3, INPUT);
  pinMode(LED_PIN_3, OUTPUT);
  pinMode(BUZZER_PIN_3, OUTPUT);
}

void loop() {
  // Measure distances from each ultrasonic sensor
  measureDistance(TRIG_PIN_1, ECHO_PIN_1, LED_PIN_1, BUZZER_PIN_1, distance_cm_1);
  measureDistance(TRIG_PIN_2, ECHO_PIN_2, LED_PIN_2, BUZZER_PIN_2, distance_cm_2);
  measureDistance(TRIG_PIN_3, ECHO_PIN_3, LED_PIN_3, BUZZER_PIN_3, distance_cm_3);

  // Print the distances to Serial Monitor
  Serial.print("Distance 1: ");
  Serial.print(distance_cm_1);
  Serial.println(" cm");

  Serial.print("Distance 2: ");
  Serial.print(distance_cm_2);
  Serial.println(" cm");

  Serial.print("Distance 3: ");
  Serial.print(distance_cm_3);
  Serial.println(" cm");

  // Read accelerometer data for fall detection
  mpu.getAcceleration(&ax, &ay, &az);
  detectFall(ax, ay, az); // Check for fall

  delay(500); // Add a delay for easier reading
}

// Function to measure distance and control LED and buzzer
void measureDistance(int trigPin, int echoPin, int ledPin, int buzzerPin, float &distance_cm) {
  // Generate a 10-microsecond pulse to the TRIG pin
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure the duration of the pulse from the ECHO pin
  float duration_us = pulseIn(echoPin, HIGH);

  // Calculate the distance in cm
  distance_cm = 0.017 * duration_us;

  // Activate LED and buzzer if distance is under limit
  if (distance_cm < 30) {
    digitalWrite(ledPin, HIGH);   // Turn on the LED (representing the motor)
    beep(buzzerPin);              // Call the function for buzzer sound pattern
  } else {
    digitalWrite(ledPin, LOW);    // Turn off the LED
    noTone(buzzerPin);            // Stop any sound from the buzzer
  }
}

// Function to create a beeping pattern for the buzzer
void beep(int buzzerPin) {
  tone(buzzerPin, 1000); // 1000 Hz tone
  delay(100);             // Beep duration (100 ms)
  noTone(buzzerPin);      // Stop the tone
  delay(100);             // Pause between beeps
}

// Function to detect falls based on accelerometer data
void detectFall(int16_t ax, int16_t ay, int16_t az) {
  // You can adjust the threshold values based on testing
  const int FALL_THRESHOLD = 15; // Example threshold for fall detection

  // Check for significant acceleration changes indicating a fall
  if (ax > FALL_THRESHOLD || ay > FALL_THRESHOLD || az > FALL_THRESHOLD) {
    Serial.println("Fall detected!");
    // Add additional actions here, like triggering a buzzer or sending a message
    tone(BUZZER_PIN_1, 1000); // Sound alarm
    delay(2000);               // Alarm duration
    noTone(BUZZER_PIN_1);      // Stop alarm
  }
}