//Acceleration Meter based locking system
//Code to unlock :
//Click on the accel meter and under accel tab,
//Put: X = -0.25g Y= 0.5g Z = -1g and click button twice
#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>

MPU6050 mpu;
Servo myServo;

const int buttonPin = 7;
int buttonState = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  myServo.attach(9);
  pinMode(buttonPin, INPUT_PULLUP); // Initialize the button pin as input with internal pull-up resistor
  myServo.write(0); // Lock Position
}

void loop() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float ax_g = ax / 16384.0; // Convert to g
  float ay_g = ay / 16384.0;
  float az_g = az / 16384.0;
  
  Serial.print("Button State: ");
  Serial.println(digitalRead(buttonPin));

  Serial.print("Accel X: ");
  Serial.print(ax_g);
  Serial.print(" g, Y: ");
  Serial.print(ay_g);
  Serial.print(" g, Z: ");
  Serial.print(az_g);
  Serial.println(" g");
  
  buttonState = digitalRead(buttonPin);
  
  if (buttonState == LOW) { // Button is pressed
    checkTilt(ax_g, ay_g, az_g);
  }
  
  delay(200); // Adjust the delay for your desired responsiveness
}

void checkTilt(float ax_g, float ay_g, float az_g) {
  if (abs(ax_g + 0.25) < 0.05 && abs(ay_g - 0.5) < 0.05 && abs(az_g + 1.0) < 0.05) {
    unlock();
  }
}

void unlock() {
  Serial.println("Unlocked!");
  myServo.write(90); // Unlock Position
  delay(5000); // Keep unlocked for 5 seconds
  myServo.write(0); // Lock back
}