#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Ultrasonic.h>
#include <MPU6050.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define TRIGGER_PIN_ULTRASONIC 15
#define ECHO_PIN_ULTRASONIC 13
Ultrasonic ultrasonic(TRIGGER_PIN_ULTRASONIC, ECHO_PIN_ULTRASONIC);

#define TRIGGER_PIN_IMU 15
#define ECHO_PIN_IMU 13
MPU6050 mpu;

void setup() {
  Serial.begin(9600);

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

  // Initialize IMU
  mpu.initialize();

  // Start Ultrasonic sensor
  pinMode(TRIGGER_PIN_ULTRASONIC, OUTPUT);
  pinMode(ECHO_PIN_ULTRASONIC, INPUT);

  // Start IMU
  pinMode(TRIGGER_PIN_IMU, OUTPUT);
  pinMode(ECHO_PIN_IMU, INPUT);
}

void loop() {
  // Read Ultrasonic sensor
  long ultrasonicDistance = ultrasonic.distanceRead();

  // Read IMU
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Display values on OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0,0);
  display.println("Ultrasonic");
  display.print("Jarak: ");
  display.print(ultrasonicDistance);
  display.println(" cm");
  display.println(" ");
  display.print("IMU Accel X: ");
  display.println(ax);
  display.print("IMU Accel Y: ");
  display.println(ay);
  display.print("IMU Accel Z: ");
  display.println(az);
  display.display();

  delay(1000); // Adjust delay as needed
}