#include <LiquidCrystal_I2C.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
sensors_event_t event;

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  Wire.begin();
    
  Serial.begin(115200);
  Serial.println(F("Hello, world !"));
  Serial.println(F("ESP32-C3 I2C"));

  Serial.println("");
  lcd.init();
  lcd.backlight();
  lcd.setCursor(3, 0);
  lcd.print("Hello, world !");
  lcd.setCursor(5, 3);
  lcd.print("ESP32-C3 I2C");
  Serial.println("LCD ready!");

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("MPU6050 ready!");


  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) { // Address 0x3d for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    // for(;;); // Don't proceed, loop forever
  }
  Serial.println("SSD1306 ready!");
  display.clearDisplay();
  display.display();
  delay(2000);
  
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(30, 10);
  display.println(F("Hello, world !"));
  display.setCursor(10, 40);
  display.println(F("ESP32-C3 I2C"));
  display.display();

  Serial.println("--------------XXX--------------");
}

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);

  Serial.print("[");
  Serial.print(millis());
  Serial.print("] X: ");
  Serial.print(event.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(event.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(event.acceleration.z);
  Serial.println(" m/s^2");
  delay(500);
}