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

MPU6050 mpu;
Servo parachuteServo;

const int servoPin = 9; // Pin connected to the servo

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  
  parachuteServo.attach(servoPin); // Attach servo to the specified pin
  
  // Uncomment the following lines to calibrate the sensor
  // mpu.calibrateAccel();
  // mpu.calibrateGyro();
  // mpu.printCalibrationResults();
}

void loop() {
  float ax, ay, az;
  float gx, gy, gz;
  
  mpu.getAcceleration(&ax, &ay, &az);
  mpu.getRotation(&gx, &gy, &gz);

  // Calculate total acceleration (magnitude)
  float totalAcceleration = sqrt(ax * ax + ay * ay + az * az);

  // Check for free fall condition based on accelerometer and gyroscope data
  if (totalAcceleration < 1.2 && (abs(gx) > 300 || abs(gy) > 300 || abs(gz) > 300)) {
    Serial.println("Free fall detected! Deploying parachute.");
    deployParachute();
    delay(1000); // Add a delay to keep the parachute deployed for a certain duration
    retractParachute();
  }

  Serial.print("Total Accel: ");
  Serial.println(totalAcceleration);
  Serial.print("Gyro X: ");
  Serial.print(gx);
  Serial.print(", Y: ");
  Serial.print(gy);
  Serial.print(", Z: ");
  Serial.println(gz);

  delay(100); // Adjust delay according to your requirement
}

void deployParachute() {
  parachuteServo.write(90); // Rotate the servo to deploy the parachute
}

void retractParachute() {
  parachuteServo.write(0); // Retract the parachute by rotating the servo to initial position
}