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

const int TRIG_PIN = 7;
const int ECHO_PIN = 6;
const int LED_PINS[] = {5, 4, 3};
const int DANGER_DIST  = 50;
const int WARNING_DIST = 100;
const unsigned long SENSE_DELAY = 500;

int oldDistance = -1;
unsigned long prevTime = 0;

Adafruit_SSD1306 oled(128, 64, &Wire, -1);

int getDistance() {
  // send trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  // calculate distance
  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * .0343) / 2;
  // return distance as an integer
  return (int)(distance + 0.5);
}

void showSplash() {
  oled.setCursor(18, 20);
  oled.print("Distance Sensor");
  oled.setCursor(50, 40);
  oled.print("V1.00");
  oled.display();
  delay(2000);
  oled.clearDisplay();
  oled.display();
}

void setup() {
  Serial.begin(115200);
  oled.setRotation(2);
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  for (int i = 0; i < 3; i++) {
    pinMode(LED_PINS[i], OUTPUT);
  }
  showSplash();
  oled.setTextSize(2);
  oled.setCursor(30, 10);
  oled.print("Range:");
  oled.display();
  Serial.println("Ready!\n");
}

void loop() {
  char dispBuffer[8];

  if (millis() - prevTime >= SENSE_DELAY) {
    prevTime = millis();
    int distance = getDistance();
    if ((distance) > (oldDistance + 2) || (distance) < (oldDistance - 2))  {
      oldDistance = distance;
      for (int led = 0; led < 3; led++) {
        digitalWrite(LED_PINS[led], LOW);
      }
      if (distance < DANGER_DIST)  {
        digitalWrite(LED_PINS[2], HIGH);
      } else if (distance < WARNING_DIST) {
        digitalWrite(LED_PINS[1], HIGH);
      } else {
        digitalWrite(LED_PINS[0], HIGH);
      }
      snprintf(dispBuffer, 8, "%3d cm", distance);
      oled.setCursor(30, 40);
      oled.print(dispBuffer);
      oled.display();
      Serial.println(dispBuffer);
    }
  }
}