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

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

#define TRIG_PIN 5
#define ECHO_PIN 18
#define SOUND_SPEED 0.034
#define PI 3.14159
Adafruit_MPU6050 mpu;;

void setup() {
  Serial.begin(115200);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  if (!mpu.begin()) {
    Serial.println("Failed to initialize MPU6050!");
    while (1);
  }

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

  display.display();
  delay(2000);
  display.clearDisplay();
}
sensors_event_t event;


void loop() {
    sensors_event_t accel, gyro, temp;
  
  // Read accelerometer, gyroscope, and temperature sensor data
  mpu.getEvent(&accel, &gyro, &temp);

  // Read distance from HC-SR04 sensor
  long duration, distance;
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(ECHO_PIN, HIGH);
  // Calculate the distance
  distance = duration * SOUND_SPEED/2;
  // Calculate the gyro
  float gyroX = gyro.gyro.x * (180.0 / PI);
  float gyroY = gyro.gyro.y * (180.0 / PI);
  float gyroZ = gyro.gyro.z * (180.0 / PI);
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print(F("Accel: X="));
  display.println(accel.acceleration.x);
  display.print(F("       Y="));
  display.println(accel.acceleration.y);
  display.print(F("       Z="));
  display.println(accel.acceleration.z);

  display.print(F("Gyro:  X="));
  display.println(gyroX);
  display.print(F("       Y="));
  display.println(gyroY);
  display.print(F("       Z="));
  display.println(gyroZ);

  display.print(F("Temp:  "));
  display.print(temp.temperature, 1); 
  display.println(F(" C"));
  
  display.print(F("Dist:  "));
  display.print(distance, 1);
  display.println(F(" cm"));

  display.display();
  Serial.print("Jarak(cm): ");
  Serial.println(distance);
  // Read MPU6050
   mpu.getAccelerometerSensor()->getEvent(&event);
    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");

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Ultrasonic Sensor:");
  display.setCursor(0, 10);
  display.print("Distance: ");
  display.print(distance);
  display.println(" cm");


  
  delay(500);
}