#define Trig 6
#define Echo 5
#include <SPI.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

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

long getTime() {
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  return pulseIn(Echo, HIGH);
}
void setup() {
  pinMode(Echo, INPUT);
  pinMode(Trig, OUTPUT);
  pinMode(2, OUTPUT);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    for (;;); // Don't proceed, loop forever
  }
}

#define charw 16
#define charh 16
static const unsigned char PROGMEM ju[] = {
  0x00, 0x00, 0x7D, 0xFE, 0x45, 0x00, 0x45, 0x00, 0x45, 0x00, 0x7D, 0xFC, 0x11, 0x04, 0x11, 0x04,
  0x5D, 0x04, 0x51, 0x04, 0x51, 0xFC, 0x51, 0x00, 0x5D, 0x00, 0xE1, 0x00, 0x01, 0xFE, 0x00, 0x00,
};
static const unsigned char PROGMEM li[] = {
  0x02, 0x00, 0x01, 0x00, 0xFF, 0xFE, 0x00, 0x00, 0x14, 0x50, 0x13, 0x90, 0x14, 0x50, 0x1F, 0xF0,
  0x01, 0x00, 0x7F, 0xFC, 0x42, 0x04, 0x44, 0x44, 0x4F, 0xE4, 0x44, 0x24, 0x40, 0x14, 0x40, 0x08,
};
void DisplayNumber(float number) { 
  display.clearDisplay();
  int i = -16;
  display.drawBitmap(i += 16, 16, ju, charw, charh, WHITE);
  display.drawBitmap(i += 16, 16, li, charw, charh, WHITE);
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(32, 16);
  display.print(":");
  display.print(number, 1);
  display.print("cm");
  display.display();
  delay(500);
}
void loop() {
  float dis = round(getTime() / 58.4767);
  if (dis > 50) {
    digitalWrite(2, HIGH);
  } else {
    digitalWrite(2, LOW);
  }
  DisplayNumber(dis);
  delay(50);
}