#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h> // Ganti header untuk sensor IMU
#include <Ultrasonic.h>

// Pin sensor ultrasonik
#define TRIGGER_PIN 18
#define ECHO_PIN 5

// Pin sensor IMU
#define MPU6050_ADDRESS 0x68

// Objek sensor ultrasonik
Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);

// Objek sensor IMU
Adafruit_MPU6050 mpu6050;

// Objek OLED
Adafruit_SSD1306 display(128, 32, &Wire, -1); // Menggunakan konstruktor yang sesuai untuk OLED 128x32

void setup() {
  // Inisialisasi serial
  Serial.begin(9600);

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

  // Inisialisasi sensor IMU
  Wire.begin();
  if (!mpu6050.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  mpu6050.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu6050.setGyroRange(MPU6050_RANGE_250_DEG);

  // Tampilkan judul
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Ultrasonik & IMU");
  display.display();
  delay(1000); // Tambahkan penundaan singkat untuk membaca judul sebelum layar kosong
  display.clearDisplay();
}

void loop() {
  // Baca nilai sensor ultrasonik
  long distance = ultrasonic.read();

  // Baca nilai sensor IMU
  sensors_event_t a, g, temp;
  mpu6050.getEvent(&a, &g, &temp);

  // Tampilkan nilai sensor ultrasonik
  display.setCursor(0, 0);
  display.print("Jarak: ");
  display.print(distance);
  display.println(" cm");

  // Tampilkan nilai sensor IMU
  display.print("Akselerometer: ");
  display.print(a.acceleration.x);
  display.print(", ");
  display.print(a.acceleration.y);
  display.print(", ");
  display.println(a.acceleration.z);

  display.print("Giroskop: ");
  display.print(g.gyro.x);
  display.print(", ");
  display.print(g.gyro.y);
  display.print(", ");
  display.println(g.gyro.z);

  display.display();

  // Tunggu sebentar sebelum membaca lagi
  delay(1000);
}