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

Adafruit_MPU6050 mpu;

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

const int trigPin = 18;
const int echoPin = 5;
//define sound speed in cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701
long duration;
float distanceCm;
float distanceInch;

// Declaration for an SSD1306 display connected to I2C (SDA, SCLpins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

void setup() {{
// put your setup code here, to run once:
Serial.begin(115200);
Serial.println("A43- Alfitho Satya");

while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");
}

pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) 
{
Serial.println(F("SSD1306 allocation failed"));
for(;;);
}
delay(500);
display.clearDisplay();
display.setTextSize(2);
display.setTextColor(WHITE);
}

void loop() {
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);


digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
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;
// Convert to inches
distanceInch = distanceCm * CM_TO_INCH;
// Prints the distance in the Serial Monitor

display.clearDisplay();
display.setCursor(0, 0);
display.print("Distance (cm): ");
display.println(distanceCm);
delay(1000);
display.display();

//ACCELERATION
display.clearDisplay();
display.setCursor(0, 0);
display.print("[");
display.print(millis());
display.print("] X: ");
display.print(a.acceleration.x);
display.print(", Y: ");
display.print(a.acceleration.y);
display.print(", Z: ");
display.print(a.acceleration.z);
display.println(" m/s^2");
delay(5000);
display.display();

//GYROSCOPE
display.clearDisplay();
display.setCursor(0, 0);
display.print("[");
display.print(millis());
display.print("] X: ");
display.print(g.gyro.x);
display.print(", Y: ");
display.print(g.gyro.y);
display.print(", Z: ");
display.print(g.gyro.z);
display.println(" m/s^2");
delay(5000);
display.display();
}