// PROGRAM menampilkan nilai sensor Ultrasonik dan IMU pada display


#include <Wire.h> 
#include <Adafruit_GFX.h> // library Adafruit GFX
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // display display width, in pixels
#define SCREEN_HEIGHT 64 // display display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, & Wire, -1);

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;

const int trigPin = 13; // read as "trigpin is an integer which is constant" with serial value 13
const int echoPin = 12; // read as "echopin is an integer which is constant" with serial value 12
//define sound speed in cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701
long duration;
float distanceCm;
float distanceInch;
String Nstring;


void setup() {
  Serial.begin(115200); // Starts the serial communication
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
    }
    Serial.println("MPU6050 ready!");

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
    }
    delay(500);
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);

    Nstring.reserve(10); 
}

void Ultrasonik(){
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);

  // mngukur durasi pulse dari pin ECHO
  long duration = pulseIn(echoPin, HIGH);
  // mwnghitung jarak
  float distanceCm = 0.017 * duration;

  Serial.print("Jarak: ");
  Serial.print(distanceCm);
  Serial.println(" cm");
  Serial.println();

  Nstring  = String(distanceCm, 2); 
  Nstring += " cm"; 
  displayCenter(Nstring);
}

void displayCenter(String text) {
  int16_t x1;
  int16_t y1;
  uint16_t width;
  uint16_t height;

  display.getTextBounds(text, 0, 0, &x1, &y1, &width, &height);

  display.clearDisplay(); 
  display.setCursor((SCREEN_WIDTH - width) / 2, (SCREEN_HEIGHT - height) / 2);
  display.println(text); 
  display.display();
}

void loop() {
  // Ultrasonik();
  // delay(3000);
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // mendefinisikan nilai ACCELERATION
  // display.clearDisplay();
  // display.setCursor(0,0);
  // Serial.println("ACCELERATION");
  // display.print("X: ");
  // Serial.print("X:");
  // display.print(a.acceleration.x);
  // Serial.print(a.acceleration.x);
  // display.println(" g");
  // Serial.println(" g");

  // display.setCursor(0,20);
  // display.print("Y: ");
  // Serial.print("Y:");
  // display.print(a.acceleration.y);
  // Serial.print(a.acceleration.y);
  // display.println(" g");
  // Serial.println(" g");

  // display.setCursor(0,40);
  // display.print("Z: ");
  // Serial.print("Z:");
  // display.print(a.acceleration.z);
  // Serial.print(a.acceleration.z);
  // display.println(" g");
  // Serial.println(" g");

  // delay(3000);
  // display.display();
  // Serial.println();

  //GYROSCOPE
  display.clearDisplay();
  display.setCursor(0, 0);
  Serial.println("GYROSCOPE");
  display.print("X:");
  Serial.print("X:");
  display.print(g.gyro.x);
  Serial.print(g.gyro.x);
  display.println(" RPS");
  Serial.println(" RPS");

  display.setCursor(0, 20);
  display.print("Y:");
  Serial.print("Y:");
  display.print(g.gyro.y);
  Serial.print(g.gyro.y);
  display.println(" RPS");
  Serial.println(" RPS");

  display.setCursor(0, 40);
  display.print("Z:");
  Serial.print("Z:");
  display.print(g.gyro.z);
  Serial.print(g.gyro.z);
  display.println(" RPS");
  Serial.println(" RPS");

  delay(3000);
  display.display();
  Serial.println();
}