#include <Wire.h>
#include <MPU6050.h>

#define TRIG_PIN 2
#define ECHO_PIN 3
#define SWITCH_PIN 5  // Slide switch
#define BUZZER_PIN 4

#define MPU_SDA 21
#define MPU_SCL 20  // Changed to GPIO 20

MPU6050 mpu;

void setup() {
    Serial.begin(115200);

    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    pinMode(SWITCH_PIN, INPUT_PULLUP);  // Pull-up to ensure stability
    pinMode(BUZZER_PIN, OUTPUT);

    Wire.begin(MPU_SDA, MPU_SCL);
    mpu.initialize();
    delay(500);  // Allow time for MPU6050 initialization

    if (!mpu.testConnection()) {
        Serial.println("MPU6050 connection failed!");
    } else {
        Serial.println("MPU6050 connected!");
    }
}

void loop() {
    if (digitalRead(SWITCH_PIN) == HIGH) {  // System ON
        Serial.println("System is ON, checking sensors...");
        
        detectObstacle();
        detectFall();
    } else {
        Serial.println("System OFF");
        digitalWrite(BUZZER_PIN, LOW);  // Ensure buzzer is OFF
    }
    delay(500);  // Small delay to avoid spamming the Serial Monitor
}

void detectObstacle() {
    long duration;
    int distance;

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

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

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

    if (distance > 0 && distance <= 2) {
        Serial.println("⚠️ Very close obstacle! High-frequency buzzer!");
        tone(BUZZER_PIN, 2000);  // High-frequency sound
    } else if (distance > 2 && distance <= 5) {
        Serial.println("⚠️ Medium distance obstacle! Medium-frequency buzzer!");
        tone(BUZZER_PIN, 1500);  // Medium-frequency sound
    } else if (distance > 5 && distance <= 10) {
        Serial.println("⚠️ Far obstacle! Low-frequency buzzer!");
        tone(BUZZER_PIN, 1000);  // Low-frequency sound
    } else {
        noTone(BUZZER_PIN);  // Turn off buzzer
    }
}

void detectFall() {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;

    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // Convert raw values to acceleration in g (assuming default sensitivity ±2g)
    float accelX = ax / 16384.0;
    float accelY = ay / 16384.0;
    float accelZ = az / 16384.0;

    // Calculate total acceleration vector
    float totalAccel = sqrt(accelX * accelX + accelY * accelY + accelZ * accelZ);

    Serial.print("Acceleration: ");
    Serial.print(totalAccel);
    Serial.println(" g");

    // If total acceleration drops below 0.5g (free fall), a fall might have occurred
    if (totalAccel < 0.5) { 
        Serial.println("⚠️ FALL DETECTED! ⚠️");
        tone(BUZZER_PIN, 2500, 500);  // Quick alert sound
        delay(1000);  // Avoid repeated triggering
    }
}