#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

// Pins
#define PIR_PIN 2          // Helmet detection
#define MQ2_PIN 34         // Alcohol detection (analog)
#define RELAY_PIN 12       // Ignition control
#define BUZZER_PIN 14      // Crash alert

int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
float ax = 0, ay = 0, az = 0, gx = 0, gy = 0, gz = 0;
boolean accident = false; //stores if a accident has occurred
boolean trigger1 = false; //stores if first trigger (lower threshold) has occurred
boolean trigger2 = false; //stores if second trigger (upper threshold) has occurred
boolean trigger3 = false; //stores if third trigger (orientation change) has occurred
byte trigger1count = 0; //stores the counts past since trigger 1 was set true
byte trigger2count = 0; //stores the counts past since trigger 2 was set true
byte trigger3count = 0; //stores the counts past since trigger 3 was set true
int angleChange = 0;

void setup() {
  Serial.begin(115200);
  
  pinMode(PIR_PIN, INPUT);
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  
  digitalWrite(RELAY_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  // Initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("MPU6050 not found. Check connections.");
    while (1);
  }

  Serial.println("Smart Helmet System Initialized.");
}

void loop() {
  bool helmetWorn = digitalRead(PIR_PIN);      // HIGH = motion detected = helmet worn
  int alcoholLevel = analogRead(MQ2_PIN);      // Simulated analog alcohol level
  bool sober = alcoholLevel < 3000;            

  if (helmetWorn && sober) {
    digitalWrite(RELAY_PIN, HIGH);             // Allow ignition
    Serial.println("Helmet worn and sober - Ignition ON");
  } else {
    digitalWrite(RELAY_PIN, LOW);              // Lock ignition
    if (!helmetWorn) Serial.println("Helmet not detected – Ignition locked");
    if (!sober) Serial.println("Alcohol detected – Ignition locked");
  }

  // Crash Detection
     
    mpu_read();
    ax = (AcX - 2050) / 16384.00;
    ay = (AcY - 77) / 16384.00;
    az = (AcZ - 1947) / 16384.00;
    gx = (GyX + 270) / 131.07;
    gy = (GyY - 351) / 131.07;
    gz = (GyZ + 136) / 131.07;
   // calculating Amplitute vactor for 3 axis
    float Raw_Amp = pow(pow(ax, 2) + pow(ay, 2) + pow(az, 2), 0.5);
    int Amp = Raw_Amp * 10;  // Mulitiplied by 10 bcz values are between 0 to 1
    Serial.println(Amp); 
    if (Amp <= 2 && trigger2 == false) { //if AM breaks lower threshold (0.4g)     
      trigger1 = true;     
      Serial.println("Level 1 Trigger");   
      }   
    if (trigger1 == true) {     
      trigger1count++;     
      if (Amp >= 12) { //if AM breaks upper threshold (3g)
        trigger2 = true;
        Serial.println("Level 2 Trigger");
        trigger1 = false; trigger1count = 0;
     }
   }
    if (trigger2 == true) {
      trigger2count++;
      angleChange = pow(pow(gx, 2) + pow(gy, 2) + pow(gz, 2), 0.5); Serial.println(angleChange);
      if (angleChange >= 30 && angleChange <= 400) { //if orientation changes by between 80-100 degrees       
        trigger3 = true; trigger2 = false; trigger2count = 0;       
        Serial.println(angleChange);       
        Serial.println("Level 3 Trigger");     
        }  
        }   
    if (trigger3 == true) {     
      trigger3count++;     
      if (trigger3count >= 10) {
        angleChange = pow(pow(gx, 2) + pow(gy, 2) + pow(gz, 2), 0.5);
       //delay(10);
        Serial.println(angleChange);
        if ((angleChange >= 0) && (angleChange <= 10)) { //if orientation changes remains between 0-10 degrees         
          accident = true; trigger3 = false; trigger3count = 0;         
          Serial.println(angleChange);       }       
        else { //user regained normal orientation         
          trigger3 = false; trigger3count = 0;         
          Serial.println("TRIGGER 3 DEACTIVATED");       }     
      }   
    }   
    if (accident == true) { //in event of a Accident Detection    
      Serial.println("ACCIDENT DETECTED");       
      accident = false;
      Serial.println("Accident Alert!! Send SMS ");
      //message_with_data = message + "Latitude = " + String(latitude, 6) + "Longitude = " + String(longitude, 6) + " Link: https://maps.google.com/maps?&z=15&mrt=yp&t=k&q=" + String(latitude, 6) + "," + String(longitude, 6);
      //modem.sendSMS(mobile_number, message_with_data);
      //message_with_data = "";
      //modem.callNumber(mobile_number);
      delay(2000);   
    }   
    if (trigger2count >= 6) { //allow 0.5s for orientation change
      trigger2 = false; trigger2count = 0;
      Serial.println("TRIGGER 2 DECACTIVATED");
   }
    if (trigger1count >= 6) { //allow 0.5s for AM to break upper threshold
      trigger1 = false; trigger1count = 0;
      Serial.println("TRIGGER 1 DECACTIVATED");
   }
    delay(1000);
  }

void mpu_read() 
{

  //Wire.beginTransmission(MPU_addr);
  //Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  //Wire.endTransmission(false);
  //Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
 }