//OLED Display Setup
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define oled_reset -1

#define screen_width 128
#define screen_height 64

#define GREEN 0x07E0
Adafruit_SSD1306 a(screen_width,screen_height, &Wire, oled_reset);
//Sensor setup
#define triggerPin 3
#define echoPin 2

int duration;
float distance_curr;
float distance_prev;
String d;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  if (!a.begin(SSD1306_SWITCHCAPVCC,0x3C)){//address of oled display
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  a.clearDisplay();
  a.setTextSize(1);
  a.setTextColor(WHITE);
  a.setCursor(0, 0);
  a.println("Initializing");
  a.display();
  delay(2000);
  a.clearDisplay();
  a.setCursor(0, 0);
  a.println("");
  a.display();
  a.clearDisplay();

  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance_curr = duration * 0.034 / 2;
  if(distance_curr!=distance_prev){
    a.clearDisplay();
  }
  d = (String) distance_curr;
  a.setCursor(0, 0);
  a.println("Distance: ");
  a.display();
  a.setCursor(0, 16);
  a.println(d + "cm");
  a.display();
  distance_prev = distance_curr;
}