//--LIBRARIES CALLING--
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
//--DISPLAY OBJECT--
#define heigth 64 
#define width  128
#define rst   -1
Adafruit_SSD1306 oled(width,heigth,&Wire,rst);
//--MPU6050 OBJECT--
Adafruit_MPU6050 mpu;

void setup() {
  
  boardBegin();
  Serial.println("Hello, XIAO ESP32-C3 BORAD!");
  oled.clearDisplay();
  oled.println("Hello, XIAO ESP32-C3 BORAD!");
  oled.display();
  delay(2000);
  
}

void loop() {
  sensors_event_t a, g, temp;

  // Read data from MPU6050 (acceleration, gyroscope, temperature)
  mpu.getEvent(&a, &g, &temp);

  // Clear the OLED display buffer
  oled.clearDisplay();
  oled.setCursor(0, 0);

  // Display acceleration data
  oled.println("ACC (m/s^2)");
  oled.print("X: "); oled.println(a.acceleration.x, 1);
  oled.print("Y: "); oled.println(a.acceleration.y, 1);
  oled.print("Z: "); oled.println(a.acceleration.z, 1);

   // Display gyroscope data
  oled.println("GYRO (rad/s)");
  oled.print("X: "); oled.println(g.gyro.x, 1);
  oled.print("Y: "); oled.println(g.gyro.y, 1);
  oled.print("Z: "); oled.println(g.gyro.z, 1);

  // Send buffer to the display (update screen)
  oled.display();

  // Small delay to control refresh rate
  delay(500);
}
void boardBegin(){
  
  //Serial and I2C Comunication, OLED Display, and IMU Sensor initialize
  Serial.begin(115200);
  Wire.begin();
  if(!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)){
    Serial.println("Oled Display NOT Found...");
    for (;;);
  }
  oled.setTextColor(WHITE);
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setCursor(5, 5);

  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    for (;;);
  }
}