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

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define SOUND_SPEED 0.034
#define PI 3.14159

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET    -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Adafruit_MPU6050 mpu;

const int trigPin = 4; // HC-SR04 Trig pin connected to Arduino pin 9
const int echoPin = 5; // HC-SR04 Echo pin connected to Arduino pin 10

void setup(void) {
  Serial.begin(115200);
  Wire.begin();

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.display(); // Show initial display buffer contents

  display.clearDisplay(); // Clear the buffer
  display.setTextSize(1); // Set text size
  display.setTextColor(SSD1306_WHITE); // Set text color to white

  if (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    while (1); // Halts the program if MPU6050 is not connected
  }
  Serial.println("MPU6050 ready!");
}

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, distanceCm;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  
  // Calculate the distance
  distanceCm = 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(distanceCm, 1);
  display.println(F(" cm"));

  display.display();
  
  Serial.println("All data are displayed on OLED");
  
  delay(1000);
}