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

// OLED settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// MPU6050 object
MPU6050 mpu;

// Button
const int buttonPin = 2;
bool showAccel = true;
bool lastButtonState = HIGH;
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 50;

void setup() {
  pinMode(buttonPin, INPUT_PULLUP); // Use internal pull-up resistor
  Wire.begin();

  // OLED init
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    while (true); // Halt on failure
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Initializing...");
  display.display();

  // MPU6050 init
  mpu.initialize();
  if (!mpu.testConnection()) {
    display.clearDisplay();
    display.println("MPU6050 failed!");
    display.display();
    while (true);
  }

  display.clearDisplay();
  display.println("MPU6050 Ready!");
  display.display();
  delay(1000);
}

void loop() {
  // Button toggle with debounce
  bool reading = digitalRead(buttonPin);
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading == LOW && lastButtonState == HIGH) {
      showAccel = !showAccel; // Toggle display mode
    }
  }
  lastButtonState = reading;

  // Read MPU6050 raw data
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convert to real-world units
  float ax_mps2 = (ax / 16384.0) * 9.81;
  float ay_mps2 = (ay / 16384.0) * 9.81;
  float az_mps2 = (az / 16384.0) * 9.81;

  float gx_dps = gx / 131.0;
  float gy_dps = gy / 131.0;
  float gz_dps = gz / 131.0;

  // Display
  display.clearDisplay();
  display.setCursor(0, 0);

  if (showAccel) {
    display.println("Accel (m/s^2):");
    display.print("X: "); display.println(ax_mps2, 2);
    display.print("Y: "); display.println(ay_mps2, 2);
    display.print("Z: "); display.println(az_mps2, 2);
  } else {
    display.println("Gyro (deg/s):");
    display.print("X: "); display.println(gx_dps, 2);
    display.print("Y: "); display.println(gy_dps, 2);
    display.print("Z: "); display.println(gz_dps, 2);
  }

  display.display();
  delay(300); // Adjust as needed
}