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

Adafruit_MPU6050 mpu;

int leftLEDs[] = {5, 4, 3, 2};  // Left bank LEDs
int centerLEDs[] = {6, 7};      // Center LEDs
int rightLEDs[] = {8, 9, 10, 11};  // Right bank LEDs

void setup(void) {
  Serial.begin(115200);

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  // Set up LEDs as outputs
  for (int i = 0; i < 4; i++) {
    pinMode(leftLEDs[i], OUTPUT);
    pinMode(rightLEDs[i], OUTPUT);
  }
  pinMode(centerLEDs[0], OUTPUT);
  pinMode(centerLEDs[1], OUTPUT);
}

void loop() {
  sensors_event_t a, g, temp;
	mpu.getEvent(&a, &g, &temp);

  // Calculate roll angle (left/right tilt)
  float roll = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;

  // Reset all LEDs
  resetLEDs();

  // Light LEDs based on roll angle

  if (roll > -5 && roll < 5) {
    // Center position
    digitalWrite(centerLEDs[0], HIGH);
    digitalWrite(centerLEDs[1], HIGH);
  } else if (roll <= -5) {
    // Left tilt: roll from -5 to -45 maps to 4 LEDs
    int level = map(roll, -45, -5, 4, 0);  // Map roll from -45 to -5 to 0-4 LEDs
    for (int i = 0; i < level; i++) {
      digitalWrite(leftLEDs[i], HIGH);  // Light up LEDs based on tilt
    }
  } else if (roll >= 5) {
    // Right tilt: roll from 5 to 45 maps to 4 LEDs
    int level = map(roll, 5, 45, 0, 4);  // Map roll from 5 to 45 to 0-4 LEDs
    for (int i = 0; i < level; i++) {
      digitalWrite(rightLEDs[i], HIGH);  // Light up LEDs based on tilt
    }
  }

  delay(100);  // Adjust for smoother LED updates
}

void resetLEDs() {
  for (int i = 0; i < 4; i++) {
    digitalWrite(leftLEDs[i], LOW);
    digitalWrite(rightLEDs[i], LOW);
  }
  digitalWrite(centerLEDs[0], LOW);
  digitalWrite(centerLEDs[1], LOW);
}