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


MPU6050 accelgyro;
Servo myservo;
const float freeFallThreshold = 0.5;

const int ledPin = 10;
const int buzzerPin = 11;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  pinMode(12,OUTPUT);
  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

  Serial.println("Testing device connections...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  myservo.attach(9);
  myservo.write(0); 

  pinMode(ledPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
  digitalWrite(ledPin, LOW);
  digitalWrite(buzzerPin, LOW);
  digitalWrite(12,HIGH);
}

void loop() {
  int16_t ax, ay, az;
  accelgyro.getAcceleration(&ax, &ay, &az);

  float axG = ax / 16384.0 * 9.81;
  float ayG = ay / 16384.0 * 9.81;
  float azG = az / 16384.0 * 9.81;

  float accelerationMagnitude = sqrt(axG * axG + ayG * ayG + azG * azG);

  Serial.print("Acceleration: ");
  Serial.println(accelerationMagnitude);

  
  if (accelerationMagnitude < freeFallThreshold) {
    Serial.println("Free fall detected!");
    deployParachute();
    signalFreeFall();
  } else {
    
    digitalWrite(ledPin, LOW);
    digitalWrite(buzzerPin, LOW);
  }

  delay(100); 
}

void deployParachute() {
  
  myservo.write(90);
  delay(2000);
  myservo.write(0); 
}

void signalFreeFall() {
  
  digitalWrite(ledPin, HIGH);
  digitalWrite(buzzerPin, HIGH);
  delay(1000); // Signal for 1 second
  digitalWrite(ledPin, LOW);
  digitalWrite(buzzerPin, LOW);
}