#include <Wire.h>
#include <U8g2lib.h>

#define TRIG_PIN 3
#define ECHO_PIN 4

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, U8X8_PIN_NONE);

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

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  Wire.begin(6, 7); // SDA, SCL
  u8g2.begin();
}

long readDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH);
  long distance = duration * 0.034 / 2;

  return distance;
}

void loop() {
  long distance = readDistance();

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  u8g2.clearBuffer();

  if (distance < 20) {
    u8g2.setFont(u8g2_font_logisoso24_tr);
    u8g2.drawStr(0, 40, "CLOSE!");
  } else {
    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.drawStr(0, 25, "Distance:");

    char buf[20];
    sprintf(buf, "%ld cm", distance);
    u8g2.drawStr(0, 50, buf);
  }

  u8g2.sendBuffer();

  delay(300);
}