#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>

#define MPU6050_ADDR 0x68 // I2C address of the MPU6050
#define LED_PIN 5         // Alert LED
#define BUZZER_PIN 4      // Buzzer
#define CAMERA_LED_PIN 14 // Simulated Camera LED

// Set the LCD address to 0x27 for a 16 chars and 2-line display
LiquidCrystal_I2C lcd(0x27, 16, 2);
float threshold = 1.5; // Adjust this threshold based on testing

void setup() {
  pinMode(LED_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(CAMERA_LED_PIN, OUTPUT);

  // Initialize the LCD
  lcd.init();
  lcd.backlight();
  
  Serial.begin(115200);
  Wire.begin();

  // Initialize the MPU6050 (wake it up from sleep mode)
  Wire.beginTransmission(MPU6050_ADDR); // Start communication with the MPU6050
  Wire.write(0x6B);  // Access the power management register
  Wire.write(0);     // Write 0 to wake up the sensor (clears sleep bit)
  Wire.endTransmission(true); // End communication

  // Display initial message
  lcd.setCursor(0, 0);
  lcd.print("System Armed");
  lcd.setCursor(0, 1);
  lcd.print("Awaiting Impact");
}

void loop() {
  int16_t accelX, accelY, accelZ;
  
  // Read accelerometer data
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B);  // Starting register for accelerometer data
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 6, true);

  accelX = Wire.read() << 8 | Wire.read(); // X-axis
  accelY = Wire.read() << 8 | Wire.read(); // Y-axis
  accelZ = Wire.read() << 8 | Wire.read(); // Z-axis

  // Calculate the magnitude of acceleration vector (converted to g)
  float accelMagnitude = sqrt(sq(accelX) + sq(accelY) + sq(accelZ)) / 16384.0; // Normalize to g units

  // Check if the acceleration exceeds the threshold (indicating an impact)
  if (accelMagnitude > threshold) {
    digitalWrite(LED_PIN, HIGH);     // Alert LED ON
    digitalWrite(BUZZER_PIN, HIGH);  // Buzzer ON
    Serial.println("Impact Detected! Alert Sent.");
    
    // Update LCD message
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Impact Detected");
    lcd.setCursor(0, 1);
    lcd.print("Sending Alert...");
    
    // Simulate camera action with LED
    digitalWrite(CAMERA_LED_PIN, HIGH);  
    Serial.println("Simulated Picture Taken.");
    delay(2000);  // Keep camera LED on for 2 seconds
    digitalWrite(CAMERA_LED_PIN, LOW);  // Turn off simulated camera LED

    // After capturing, show a different message
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Pic Captured!");
    lcd.setCursor(0, 1);
    lcd.print("Owner Notified.");
    
  } else {
    digitalWrite(LED_PIN, LOW);      // Alert LED OFF
    digitalWrite(BUZZER_PIN, LOW);   // Buzzer OFF

    // Return to default message when no impact is detected
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("System Armed");
    lcd.setCursor(0, 1);
    lcd.print("Awaiting Impact");
  }
  delay(1000); // Adjust delay for sensitivity
}