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

// ---------- PINS ----------
const int MEASURE_BTN = 2;
const int SCAN_BTN    = 4;

const int TRIG_PIN = 9;
const int ECHO_PIN = 10;

// ---------- OLED ----------
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define OLED_ADDRESS 0x3C

Adafruit_SSD1306 display(
  SCREEN_WIDTH,
  SCREEN_HEIGHT,
  &Wire,
  OLED_RESET
);

// ---------- STATE ----------
float distanceCm = -1;

bool scanning = false;

bool lastMeasureState = HIGH;
bool lastScanState    = HIGH;

unsigned long lastScanTime = 0;

const unsigned long SCAN_INTERVAL = 150;


// ---------- SENSOR ----------
float measureDistance() {

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

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

  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);

  if (duration == 0) {
    return -1;
  }

  return duration * 0.0343 / 2.0;
}


// ---------- DISPLAY ----------
void updateDisplay(const char* status) {

  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE);

  // Title
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.println("DISTANCE TOOL");

  display.drawLine(0, 10, 127, 10, SSD1306_WHITE);

  // Distance
  display.setTextSize(2);
  display.setCursor(0, 22);

  if (distanceCm < 0) {
    display.println("---");
  }
  else {
    display.print(distanceCm, 1);
    display.println(" cm");
  }

  // Status
  display.setTextSize(1);
  display.setCursor(0, 50);
  display.println(status);

  display.display();
}


// ---------- SETUP ----------
void setup() {

  // Buttons
  pinMode(MEASURE_BTN, INPUT_PULLUP);
  pinMode(SCAN_BTN, INPUT_PULLUP);

  // Ultrasonic sensor
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  digitalWrite(TRIG_PIN, LOW);

  // OLED
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS);

  // Initial screen
  updateDisplay("READY");
}


// ---------- LOOP ----------
void loop() {

  // =========================
  // MEASURE BUTTON
  // =========================

  bool measureState = digitalRead(MEASURE_BTN);

  if (lastMeasureState == HIGH && measureState == LOW) {

    distanceCm = measureDistance();

    updateDisplay("MEASURE");

    delay(30);
  }

  lastMeasureState = measureState;


  // =========================
  // SCAN BUTTON
  // =========================

  bool scanState = digitalRead(SCAN_BTN);

  if (lastScanState == HIGH && scanState == LOW) {

    scanning = !scanning;

    if (scanning) {
      updateDisplay("SCANNING...");
    }
    else {
      updateDisplay("READY");
    }

    delay(30);
  }

  lastScanState = scanState;


  // =========================
  // CONTINUOUS SCAN
  // =========================

  if (scanning && millis() - lastScanTime >= SCAN_INTERVAL) {

    lastScanTime = millis();

    distanceCm = measureDistance();

    updateDisplay("SCANNING...");
  }
}