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

MPU6050 mpu;

// Pin definitions
const int centerLEDs[] = {2, 3, 4, 5, 6};  // Y-Axis and Center Lock
const int leftLEDs[]   = {7, 8, 9};        // X-Axis Left Tilt
const int rightLEDs[]  = {10, 11, 12};     // X-Axis Right Tilt
const int buzzerPin    = 13;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();

  // Connection check
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1); 
  }

  // Initialize all LED pins as outputs
  for (int i = 0; i < 5; i++) pinMode(centerLEDs[i], OUTPUT);
  for (int i = 0; i < 3; i++) {
    pinMode(leftLEDs[i], OUTPUT);
    pinMode(rightLEDs[i], OUTPUT);
  }
  pinMode(buzzerPin, OUTPUT);
}

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

  // Calculate tilt angles in degrees using trigonometry
  float xAngle = atan2(ax, az) * 180.0 / PI;
  float yAngle = atan2(ay, az) * 180.0 / PI;

  // Debugging output to Serial Monitor
  Serial.print("X-Tilt: "); Serial.print(xAngle);
  Serial.print(" | Y-Tilt: "); Serial.println(yAngle);

  clearAllLEDs();

  // --- LEVEL LOCK LOGIC ---
  // Middle LED ON + Buzzer if nearly level on both axes (within 5 degrees)
  if (abs(xAngle) < 5 && abs(yAngle) < 5) {
    digitalWrite(centerLEDs[2], HIGH); // The "Perfect Level" LED
    digitalWrite(buzzerPin, HIGH);
  } else {
    digitalWrite(buzzerPin, LOW);

    // --- X-AXIS (LEFT/RIGHT) LED CONTROL ---
    if (xAngle < -5) {
      if (xAngle < -30)      digitalWrite(leftLEDs[2], HIGH); // Steep Left
      else if (xAngle < -20) digitalWrite(leftLEDs[1], HIGH); // Mid Left
      else                   digitalWrite(leftLEDs[0], HIGH); // Slight Left
    }
    if (xAngle > 5) {
      if (xAngle > 30)      digitalWrite(rightLEDs[2], HIGH); // Steep Right
      else if (xAngle > 20) digitalWrite(rightLEDs[1], HIGH); // Mid Right
      else                  digitalWrite(rightLEDs[0], HIGH); // Slight Right
    }

    // --- Y-AXIS (FRONT/BACK) LED CONTROL ---
    if (yAngle < -10)      digitalWrite(centerLEDs[0], HIGH); // Steep Back
    else if (yAngle < -5)  digitalWrite(centerLEDs[1], HIGH); // Slight Back
    else if (yAngle > 10)  digitalWrite(centerLEDs[4], HIGH); // Steep Front
    else if (yAngle > 5)   digitalWrite(centerLEDs[3], HIGH); // Slight Front
  }

  delay(100); // 10Hz Refresh rate for stability
}

// Function to reset all indicators before next reading
void clearAllLEDs() {
  for (int i = 0; i < 5; i++) digitalWrite(centerLEDs[i], LOW);
  for (int i = 0; i < 3; i++) {
    digitalWrite(leftLEDs[i], LOW);
    digitalWrite(rightLEDs[i], LOW);
  }
}