#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_MPU6050.h>
#include <Ultrasonic.h>
#include <ESP32Servo.h> // Include the ESP32Servo library

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

// Pin sensor IMU
#define MPU6050_ADDRESS 0x68

// Pin untuk servo
#define SERVO_PIN 9

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

// Objek sensor IMU
Adafruit_MPU6050 mpu6050;

// Objek OLED
Adafruit_SSD1306 display(128, 32, &Wire, -1);

// Objek servo
Servo servo; // Change to ESP32Servo

void setup() {
  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);

  // Inisialisasi servo
  servo.attach(SERVO_PIN);

  // Tampilkan judul
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Ultrasonik & IMU");
  display.display();
  delay(1000);
  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);

  // Kontrol debit air berdasarkan jarak yang terdeteksi
  if (distance > 0 && distance < 30) {
    servo.write(90); // Mengatur posisi servo untuk debit air
  } else {
    servo.write(0); // Tutup debit air jika tidak ada objek yang terdeteksi
  }

  // 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();

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