// #include <Adafruit_GFX.h>
// #include <Adafruit_SSD1306.h>
// #include <BluetoothSerial.h>

// #define SCREEN_WIDTH 128 // OLED display width, in pixels
// #define SCREEN_HEIGHT 64 // OLED display height, in pixels

// // Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// #define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
// Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// // Bluetooth setup
// BluetoothSerial SerialBT;

// // Button pins
// #define NEXT_BUTTON 13
// #define SELECT_BUTTON 14
// #define PREVIOUS_BUTTON 12
// // #define MENU_BUTTON 4

// // Variables
// bool menuShow = false;
// byte menuSelection = 0;

// // Icons



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

//   // // Bluetooth initialization
//   SerialBT.begin("ESP32-BT");
//   Serial.println("Bluetooth started");


//   // Buttons setup
//   pinMode(NEXT_BUTTON, INPUT_PULLUP);
//   pinMode(SELECT_BUTTON, INPUT_PULLUP);
//   pinMode(PREVIOUS_BUTTON, INPUT_PULLUP);
//   // pinMode(MENU_BUTTON, INPUT_PULLUP);

//   // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
//   if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
//     Serial.println(F("SSD1306 allocation failed"));
//     for(;;); // Don't proceed, loop forever
//   }

//   // Show initial display buffer contents on the screen --
//   // the library initializes this with an Adafruit splash screen.
//   display.display();
//   delay(200); // Pause for 2 seconds

//   drawStartUp();
//   delay(200); 

//   menuSelection = 0;
//   // drawStartUp();
//   drawMenu();
// }

// void loop() {
//   // Handle Bluetooth messages
//   if (SerialBT.available()) {
//     String received = SerialBT.readString();
//     Serial.println("Received: " + received);
//     if (received.startsWith("MSG:")) {
//       String message = received.substring(4);
//       display.clearDisplay();
//       display.setCursor(0, 0);
//       display.println("Message Received:");
//       display.setCursor(0, 10);
//       display.println(message);
//       display.display();
//     }
//   }

//   // Handle buttons
//   if (digitalRead(NEXT_BUTTON) == LOW) {
//     menuSelection = (menuSelection + 1) % 3;
//     drawMenu();
//     delay(200); // Debounce
//   } else if (digitalRead(PREVIOUS_BUTTON) == LOW) {
//     menuSelection = (menuSelection + 2) % 3; // Avoid negative values
//     drawMenu();
//     delay(200); // Debounce
//   } else if (digitalRead(SELECT_BUTTON) == LOW) {
//     display.clearDisplay();
//     display.setCursor(0, 0);
//     display.print("Selected: Option ");
//     display.println(menuSelection + 1);
//     display.display();
//     delay(1000); // Display selection
//     drawMenu();
//   }
// }

// void drawStartUp() {
//   display.clearDisplay();
//   display.setTextSize(1);
//   display.setTextColor(SSD1306_WHITE);
//   display.setCursor(0, 0);
//   display.println("Initializing...");
//   display.display();
//   delay(1000);
// }

// void drawMenu() {
//   display.clearDisplay();
//   display.setCursor(0, 0);
//   display.println("Menu:");
//   display.setCursor(0, 10);
//   display.println(menuSelection == 0 ? "> Option 1" : "  Option 1");
//   display.setCursor(0, 20);
//   display.println(menuSelection == 1 ? "> Option 2" : "  Option 2");
//   display.setCursor(0, 30);
//   display.println(menuSelection == 2 ? "> Option 3" : "  Option 3");
//   display.display();
// }


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

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Adafruit_MPU6050 mpu;

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

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

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  Serial.println("SSD1306 ready!");

  display.display();
  delay(200); // Pause for 2 seconds

  drawStartUp();
  delay(200); 
}

sensors_event_t event;

void loop() {
  display.clearDisplay();

  mpu.getAccelerometerSensor()->getEvent(&event);
  display.setCursor(0, 0);
  display.print("[");
  display.print(millis());
  display.print("] X: ");
  display.print(event.acceleration.x);
  display.print(", Y: ");
  display.print(event.acceleration.y);
  display.print(", Z: ");
  display.print(event.acceleration.z);
  display.println(" m/s^2");
  display.display();
  delay(500);
}

void drawStartUp() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Initializing...");
  display.display();
  delay(1000);
}