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

MPU6050 imu;
LiquidCrystal_I2C lcd(0x27, 16, 2);

// LED pins
const int R = 4;                  // red  → backward tilt
const int G = 3;                  // green → forward tilt
const int B = 2;                  // blue → left / right tilt

// Thresholds in raw accelerometer units
const int16_t TILT  = 10000;          // tilt detection
const int32_t SHAKE = 16000L * 16000L;  // shake detection (squared)

void setup() {
  Wire.begin();                   // start I2C
  imu.initialize();
  pinMode(R, OUTPUT);
  pinMode(G, OUTPUT);
  pinMode(B, OUTPUT);

  lcd.init();
  lcd.backlight();
  lcd.print("MPU6050 Ready");
  delay(500);
  lcd.clear();
}

void loop() {
  int16_t ax, ay, az;
  imu.getAcceleration(&ax, &ay, &az);

  // shake detection
  int32_t magSq = (int32_t)ax * ax + (int32_t)ay * ay + (int32_t)az * az;
  if (magSq > SHAKE) {
    lcd.clear();
    lcd.print("Shake Detected!");
    blinkAll();
    lcd.clear();
    return;                       // skip tilt handling on this loop
  }

  lcd.setCursor(0, 0);
  lcd.print("Ax:"); lcd.print(ax);
  lcd.print(" Ay:"); lcd.print(ay);

  lcd.setCursor(0, 1);
  digitalWrite(R, LOW);
  digitalWrite(G, LOW);
  digitalWrite(B, LOW);

  if (ay > TILT) {                // forward
    lcd.print("Tilt: Forward ");
    digitalWrite(G, HIGH);
  }
  else if (ay < -TILT) {          // backward
    lcd.print("Tilt: Backward");
    digitalWrite(R, HIGH);
  }
  else if (ax > TILT) {           // right
    lcd.print("Tilt: Right   ");
    digitalWrite(B, HIGH);
  }
  else if (ax < -TILT) {          // left
    lcd.print("Tilt: Left    ");
    digitalWrite(B, HIGH);
  }
  else {                          // stable
    lcd.print("Tilt: Stable  ");
  }

  delay(100);                     // refresh rate
}

// Blink all three LEDs five times
void blinkAll() {
  for (int i = 0; i < 5; i++) {
    digitalWrite(R, HIGH);
    digitalWrite(G, HIGH);
    digitalWrite(B, HIGH);
    delay(100);
    digitalWrite(R, LOW);
    digitalWrite(G, LOW);
    digitalWrite(B, LOW);
    delay(100);
  }
}