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

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

// Definisi untuk HC-SR04
#define TRIG_PIN 25
#define ECHO_PIN 33

// Objek MPU6050
Adafruit_MPU6050 mpu;

void setup() {
  // Inisialisasi Serial Monitor
  Serial.begin(115200);

  // Inisialisasi OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("Gagal menginisialisasi OLED"));
    for (;;);
  }
  display.clearDisplay();

  // Inisialisasi HC-SR04
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Inisialisasi MPU6050
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  // Tampilkan pesan awal di OLED
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("HC-SR04 & MPU6050");
  display.println("Reading...");
  display.display();
  delay(2000);
}

void loop() {
  // Membaca jarak dari HC-SR04
  long duration, distance;
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;

  // Membaca data dari MPU6050
  sensors_event_t event;
  mpu.getAccelerometerSensor()->getEvent(&event);

  // Tampilkan data di Serial Monitor
  Serial.print("Jarak: ");
  Serial.print(distance);
  Serial.println(" cm");

  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");

  // Tampilkan di OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("HC-SR04 & MPU6050");
  display.setCursor(0, 10);
  display.print("Jarak: ");
  display.print(distance);
  display.println(" cm");

  display.setCursor(0, 20);
  display.print("Acc X: ");
  display.print(event.acceleration.x);
  display.println(" m/s^2");

  display.setCursor(0, 30);
  display.print("Acc Y: ");
  display.print(event.acceleration.y);
  display.println(" m/s^2");

  display.setCursor(0, 40);
  display.print("Acc Z: ");
  display.print(event.acceleration.z);
  display.println(" m/s^2");

  display.display();

  delay(500); // Tunggu sebentar sebelum pembacaan berikutnya
}