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

#define MPU 0x68

#define GREEN 11
#define YELLOW 9
#define RED 7
#define BUZZER 3

int16_t ax, ay, az;

void setup() {

  Serial.begin(9600);
  Wire.begin();

  // Wake up MPU6050
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission();

  pinMode(GREEN, OUTPUT);
  pinMode(YELLOW, OUTPUT);
  pinMode(RED, OUTPUT);
  pinMode(BUZZER, OUTPUT);

  Serial.println("Accident Detection Started");
}

void loop() {

  // Read MPU6050
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);

  Wire.requestFrom(MPU, 6);

  ax = Wire.read() << 8 | Wire.read();
  ay = Wire.read() << 8 | Wire.read();
  az = Wire.read() << 8 | Wire.read();

  // Convert raw values to g
  float x = ax / 16384.0;
  float y = ay / 16384.0;
  float z = az / 16384.0;

  // Calculate total acceleration
  float acceleration = sqrt(x * x + y * y + z * z);

  Serial.print("Acceleration: ");
  Serial.print(acceleration);
  Serial.println(" g");

  // Turn LEDs OFF first
  digitalWrite(GREEN, LOW);
  digitalWrite(YELLOW, LOW);
  digitalWrite(RED, LOW);

  // Decide severity

  if (acceleration >= 2.0) {

    // CRASH
    Serial.println("!!! CRASH !!!");

    digitalWrite(RED, HIGH);

    // Buzzer ON for 3 seconds
    tone(BUZZER, 500);
    delay(3000);

    noTone(BUZZER);

    digitalWrite(RED, LOW);
  }

  else if (acceleration >= 1.4) {

    // SUDDEN MOVEMENT
    digitalWrite(YELLOW, HIGH);

    Serial.println("SUDDEN MOVEMENT");

  }

  else {

    // NORMAL
    digitalWrite(GREEN, HIGH);

    Serial.println("NORMAL");
  }

  delay(200);
}