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

// LED configuration
#define NUM_LEDS 64
#define DATA_PIN D7
#define LED_TYPE WS2812B
#define COLOR_ORDER GRB
#define MATRIX_WIDTH 8
#define MATRIX_HEIGHT 8

// Display parameters
#define BRIGHTNESS 220         // Overall brightness
#define LEVEL_COLOR CRGB::Green
#define BUBBLE_COLOR CRGB::Red

// Motion sensitivity parameters
#define TILT_SENSITIVITY 0.5   // How much tilt affects level display

// Initialize FastLED
CRGB leds[NUM_LEDS];

// Initialize MPU6050
Adafruit_MPU6050 mpu;

// Motion variables
float pitch = 0;
float roll = 0;

void setup() {
  Serial.begin(115200);
  Serial.println("Digital Level with Motion Sensor Starting");

  // Initialize I2C
  Wire.begin();

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

  // Configure motion sensor
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Initialize LEDs
  FastLED.addLeds<LED_TYPE, DATA_PIN, COLOR_ORDER>(leds, NUM_LEDS);
  FastLED.setBrightness(BRIGHTNESS);
  FastLED.clear();
  FastLED.show();
}

// Convert x,y coordinates to LED index with zigzag starting from top right
uint16_t XY(uint8_t x, uint8_t y) {
  uint16_t i;
  // Start from right top with x flipped
  x = MATRIX_WIDTH - 1 - x;

  if (x & 0x01) {  // Odd columns run downwards
    i = (x * MATRIX_HEIGHT) + y;
  } else {         // Even columns run upwards
    i = (x * MATRIX_HEIGHT) + (MATRIX_HEIGHT - 1 - y);
  }
  return i;
}

// Update orientation from sensor data
void updateOrientation() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Calculate pitch and roll from accelerometer data
  pitch = atan2(a.acceleration.y, a.acceleration.z) * 180.0 / PI;
  roll = atan2(-a.acceleration.x, sqrt(a.acceleration.y * a.acceleration.y +
                                     a.acceleration.z * a.acceleration.z)) * 180.0 / PI;

  // Print for debugging
  Serial.print("Pitch: ");
  Serial.print(pitch);
  Serial.print(", Roll: ");
  Serial.println(roll);
}

// Display level indicator based on orientation
void displayLevel() {
  // Clear all LEDs
  FastLED.clear();

  // Map roll and pitch values to our 8x8 grid
  int xBubble = constrain(map(roll, -30, 30, 0, MATRIX_WIDTH-1), 0, MATRIX_WIDTH-1);
  int yBubble = constrain(map(pitch, -30, 30, 0, MATRIX_HEIGHT-1), 0, MATRIX_HEIGHT-1);

  // Display center crosshair (represents level position)
  for (int x = 0; x < MATRIX_WIDTH; x++) {
    leds[XY(x, MATRIX_HEIGHT/2)] = CRGB(30, 30, 30); // Dim white for horizontal line
  }
  for (int y = 0; y < MATRIX_HEIGHT; y++) {
    leds[XY(MATRIX_WIDTH/2, y)] = CRGB(30, 30, 30); // Dim white for vertical line
  }

  // Display bubble (current position)
  leds[XY(xBubble, yBubble)] = BUBBLE_COLOR;

  // Make the bubble brighter if it's close to level
  if (abs(xBubble - MATRIX_WIDTH/2) <= 1 && abs(yBubble - MATRIX_HEIGHT/2) <= 1) {
    // Brighten the center spot
    leds[XY(MATRIX_WIDTH/2, MATRIX_HEIGHT/2)] = CRGB::Yellow;

    // If we're exactly level, show a special indicator
    if (xBubble == MATRIX_WIDTH/2 && yBubble == MATRIX_HEIGHT/2) {
      // Perfect level - brighten the center
      leds[XY(MATRIX_WIDTH/2, MATRIX_HEIGHT/2)] = CRGB::White;
    }
  }
}

void loop() {
  // Update orientation from motion sensor
  updateOrientation();

  // Display the level visualization
  displayLevel();

  // Update display
  FastLED.show();

  // Small delay for animation smoothness
  delay(30);
}