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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SOUND_SPEED 0.034
#define PI 3.14159
#define OLED_RESET -1

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_MPU6050 mpu;
const int trigPin = 4, echoPin = 5;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { Serial.println(F("SSD1306 allocation failed")); for (;;); }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  if (!mpu.begin()) { Serial.println("MPU6050 not connected!"); while (1); }
  Serial.println("MPU6050 ready!");
}

void loop() {
  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);
  long duration, distanceCm;
  digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distanceCm = duration * SOUND_SPEED/2;
  float gyroX = gyro.gyro.x * (180.0 / PI), gyroY = gyro.gyro.y * (180.0 / PI), 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(distanceCm, 1); display.println(F(" cm"));
  display.display();
  Serial.println("All data are displayed on OLED");
  delay(1000);
}