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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1  // Reset pin # (or -1 if sharing Arduino reset pin)
#define BUTTON_PIN 18

MPU6050 mpu;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

bool measuring = false;

void setup() {
    Serial.begin(115200);
    pinMode(BUTTON_PIN, INPUT_PULLUP);  // Button setup with pull-up resistor

    // Initialize MPU6050 sensor
    Wire.begin();
    mpu.initialize();

    // Check if MPU6050 is connected
    if (!mpu.testConnection()) {
        Serial.println("MPU6050 connection failed");
        while (1);
    } else {
        Serial.println("MPU6050 connected successfully");
    }

    // Initialize OLED display with I2C address 0x3C
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("SSD1306 allocation failed"));
        while (1);
    }

    // Clear the buffer and print initial message
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.println("MPU6050 Ready");
    display.display();
}

void loop() {
    if (digitalRead(BUTTON_PIN) == LOW) {
        if (!measuring) {
            measuring = true;
            display.clearDisplay();
            display.setCursor(0, 0);
            display.println("Measuring...");
            display.display();
        }
    }

    if (measuring) {
        int16_t ax, ay, az;
        int16_t gx, gy, gz;

        // Get acceleration and gyroscope data
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

        // Clear display and print the data
        display.clearDisplay();
        display.setTextSize(1);
        display.setCursor(0, 0);

        display.println("Accel:");
        display.print("X: "); display.println(ax);
        display.print("Y: "); display.println(ay);
        display.print("Z: "); display.println(az);

        display.setCursor(0, 32);
        display.println("Gyro:");
        display.print("X: "); display.println(gx);
        display.print("Y: "); display.println(gy);
        display.print("Z: "); display.println(gz);

        display.display();  // Update the display

        delay(1000);  // Adjust delay as needed

        // If the button is released, stop measuring
        if (digitalRead(BUTTON_PIN) == HIGH) {
            Serial.println("Stop measuring physical parameters.");
            measuring = false;
            display.clearDisplay();
            display.setCursor(0, 0);
            display.println("Measurement Stopped");
            display.display();
        }
    }
}