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

// 1.Inisialisasi dan Konfigurasi:
// Mendefinisikan Layar OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Mendefinisikan Pin Sensor Ultrasonik dan Kecepatan Suara
#define TRIGGER_PIN 5
#define ECHO_PIN 18
#define SOUND_SPEED 0.034

// Membuat objek untuk sensor MPU6050 dan display OLED
Adafruit_MPU6050 mpu;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

long duration;
float distanceCm;

// 2. Setup
// Menginisialisasi MPU6050 dan OLED. Jika gagal, program akan berhenti
void setup() {
  Serial.begin(115200);
  Wire.begin();

  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  if (!mpu.begin()) {
    Serial.println("MPU6050 tidak terdeteksi");
    while (1);
  }

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 tidak terdeteksi");
    while (1);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Sensor Data");
  display.display();
  delay(2000);
}

// 3. Fungsi untuk menghitung jarak
// Mengirim pulsa ultrasonik dan menghitung jarak berdasarkan waktu kembali.
float getDistance() {
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  
  duration = pulseIn(ECHO_PIN, HIGH);
  distanceCm = duration * SOUND_SPEED / 2;
  
  return distanceCm;
}

// 4. Loop Utama
void loop() {
  float distance = getDistance();

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  // Kode untuk menampilkan data Jarak dari Sensor Ultrasonik ke OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Distance: ");
  display.print(distance, 1);
  display.println(" cm \n");
  
  // Kode untuk menampilkan data Accelerometer dan Gyroscope dari Sensor IMU/MPU ke OLED
  display.println("Accel | Gyros");
  display.print("X:");
  display.print(a.acceleration.x, 1);
  display.print(" | ");
  display.println(g.gyro.x, 1);
  
  display.print("Y:");
  display.print(a.acceleration.y, 1);
  display.print(" | ");
  display.println(g.gyro.y, 1);
  
  display.print("Z:");
  display.print(a.acceleration.z, 1);
  display.print(" | ");
  display.println(g.gyro.z, 1);
  
  display.display();

  // Mencetak data ke Serial MOnitor
  Serial.print("Distance (cm): ");
  Serial.println(distance);
  Serial.print("Accel X: ");
  Serial.print(a.acceleration.x);
  Serial.print(" Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(" Z: ");
  Serial.println(a.acceleration.z);
  Serial.print("Gyro X: ");
  Serial.print(g.gyro.x);
  Serial.print(" Y: ");
  Serial.print(g.gyro.y);
  Serial.print(" Z: ");
  Serial.println(g.gyro.z);
  Serial.println();

  delay(100);
}