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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

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

// Визначення пінів для ультразвукових датчиків
int trigPin1 = 9;
int echoPin1 = 8;
int trigPin2 = 3;
int echoPin2 = 2;
int trigPin3 = 5;
int echoPin3 = 4;

// Швидкість звуку в повітрі при 20°C
float v = 331.5 + 0.6 * 20;

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

  // Налаштування пінів датчиків
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);

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

  //  Показати логотип на OLED екрані
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.display();
  delay(1000);
  display.clearDisplay();
}

void loop() {
  // Отримання відстані від кожного датчика
  float dist1 = getDistance(trigPin1, echoPin1);
  float dist2 = getDistance(trigPin2, echoPin2);
  float dist3 = getDistance(trigPin3, echoPin3);

  // Виведення результатів на OLED дисплей
  printResult("S1: ", dist1, " cm", 10, 10, false);
  printResult("S2: ", dist2, " cm", 10, 30, false);
  printResult("S3: ", dist3, " cm", 10, 50, true);
 
  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);
  // переводимо мкс в с (ділимо на 1000000) і відкидаэмо час витрачений на зворотний шлях (ділимо на 2)
  float t = tu / 2000000;
  // дистанція в см
  float dist = t * v * 100;

  return dist;
}

// Функція для виведення результатів на OLED дисплей
void printResult(String scrinTxt, float data, String scrDisttxt, int sx, int sy, bool end) {
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(sx,sy);
  display.print(scrinTxt);
  display.print(data);
  display.println(scrDisttxt);

  if (end) {
    display.display();
    display.clearDisplay();
  }
}