#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define LOGO16_GLCD_WIDTH  128
#define LOGO16_GLCD_HEIGHT 32

#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(LOGO16_GLCD_WIDTH, LOGO16_GLCD_HEIGHT, &Wire, OLED_RESET);

#if (SSD1306_LCDHEIGHT != LOGO16_GLCD_HEIGHT)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif

int trigPin = 5;
int echoPin = 4;

float v = 331.5 + 0.6 * 20;

void setup() {
  // Serial.begin(9600);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    
    for(;;);
  }
  
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();
  delay(2000);
  display.clearDisplay();
}

void loop() {
  float dist = getDistance(trigPin, echoPin);
  
  // Serial.println(dist);

  display.clearDisplay();
  robojaxText(5, 5, 1, false, dist);
  display.display();

  delay(100);
}

float getDistance(int tpin, int epin) {
  digitalWrite(tpin, LOW);
  delayMicroseconds(3);
  digitalWrite(tpin, HIGH);
  delayMicroseconds(5);
  digitalWrite(tpin, LOW);

  float tu = pulseIn(epin, HIGH);
  float t = tu / 2000000;
  float dist = t * v * 100;

  return dist;
}

void robojaxText(int x, int y, int size, boolean d, float dist) {
  display.setTextSize(size);
  display.setTextColor(WHITE);
  display.setCursor(x,y);
  display.print("DISTANCE: ");
  display.print(dist);
  display.println(" cm");
  display.println();

  if(d){
    display.display();
  }
}