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

// Määritä OLED-näytön reset-pin ja I2C-osoite
#define OLED_RESET -1
Adafruit_SSD1306 display(OLED_RESET, 0x3C);

const int trigPin = GPIO_NUM_6;
const int echoPin = GPIO_NUM_5;

// Määritä äänen nopeus cm/uS ja muunnoskerroin tuumiksi
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701

long duration;
int distanceCm;
int distanceInch;

void setup() {
  Serial.begin(115200);

  // Aseta trig ja echo pinnien tilat
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Käynnistä OLED-näyttö
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED-näytön alustus epäonnistui"));
    for (;;) {}
  }
  delay(500);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
}

void loop() {
  // Nollaa trig-pinni
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);
  
  // Lähetä trig-pulssi
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Mittaa echo-signaalin kesto
  duration = pulseIn(echoPin, HIGH);

  // Laske etäisyys cm
  distanceCm = duration * SOUND_SPEED / 2;

  // Muunna etäisyys tuumiksi
  distanceInch = distanceCm * CM_TO_INCH;

  // Tulosta etäisyys sarjamonitoriin
  Serial.print("Etäisyys (cm): ");
  Serial.println(distanceCm);
  Serial.print("Etäisyys (tuuma): ");
  Serial.println(distanceInch);

  // Näytä etäisyys OLED-näytössä
  display.clearDisplay();
  display.setCursor(10, 2);
  display.print("Etäisyys");
  display.setCursor(10, 15);
  display.print(distanceCm);
  display.print(" cm");
  display.setCursor(10, 23);
  display.print(distanceInch);
  display.print(" tuuma");
  display.display();

  delay(500);
}