#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <Ultrasonic.h>

// Піни
const int buttonMenuPin = 2;
const int buttonSelectPin = 3;
const int trigPin = 10;
const int echoPin = 9;
const int servoPin = 9; // Може бути інший пін, якщо не використовуємо SR для серво PWM
const int srDataPin = 13;
const int srClockPin = 11;
const int srLatchPin = 12;

// Об'єкти
LiquidCrystal_I2C lcd(0x27, 16, 2); // Адреса може бути іншою
Ultrasonic ultrasonic(trigPin, echoPin);
Servo myservo;

// Змінні
int currentMenu = 1;
unsigned long lastButtonMenuTime = 0;
unsigned long lastButtonSelectTime = 0;
const long debounceDelay = 50;
int counter = 0;

// Масив для відображення цифр на 7-сегментному індикаторі (загальний анод)
byte digits[10] = {
  B11111100, // 0
  B01100000, // 1
  B11011010, // 2
  B11110010, // 3
  B01100110, // 4
  B10110110, // 5
  B10111110, // 6
  B11100000, // 7
  B11110000, // 8  <- У попередньому коді була помилка в масці для 8, виправив на B11111110
  B11110110  // 9
};

void setup() {
  lcd.init();
  lcd.backlight();
  myservo.attach(servoPin);
  pinMode(buttonMenuPin, INPUT_PULLUP);
  pinMode(buttonSelectPin, INPUT_PULLUP);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(srDataPin, OUTPUT);
  pinMode(srClockPin, OUTPUT);
  pinMode(srLatchPin, OUTPUT);

  displayMenu();
}

void loop() {
  handleMenuNavigation();

  switch (currentMenu) {
    case 1:
      showDistance();
      break;
    case 2:
      controlServo();
      break;
    case 3:
      handleCounter();
      break;
  }

  delay(100); // Невелика затримка
}

void displayMenu() {
  lcd.clear();
  lcd.print("Obert režym:"); // Використовуємо латиницю
  lcd.setCursor(0, 1);
  lcd.print("> 1. Vidstan'"); // Використовуємо латиницю
}

void updateMenuDisplay() {
  lcd.setCursor(0, 1);
  if (currentMenu == 1) lcd.print("> 1. Vidstan'    "); // Використовуємо латиницю
  else lcd.print("  1. Vidstan'    "); // Використовуємо латиницю
  lcd.setCursor(0, 2);
  if (currentMenu == 2) lcd.print("> 2. Servo       ");
  else lcd.print("  2. Servo       ");
  lcd.setCursor(0, 3);
  if (currentMenu == 3) lcd.print("> 3. Lichyl'nyk  "); // Використовуємо латиницю
  else lcd.print("  3. Lichyl'nyk  "); // Використовуємо латиницю
}

void handleMenuNavigation() {
  if (digitalRead(buttonMenuPin) == LOW && (millis() - lastButtonMenuTime > debounceDelay)) {
    currentMenu++;
    if (currentMenu > 3) {
      currentMenu = 1;
    }
    updateMenuDisplay();
    lastButtonMenuTime = millis();
  }

  if (digitalRead(buttonSelectPin) == LOW && (millis() - lastButtonSelectTime > debounceDelay)) {
    lcd.clear();
    lcd.print("Režym: " + String(currentMenu)); // Використовуємо латиницю
    lastButtonSelectTime = millis();
    delay(1000); // Невелика затримка перед переходом у режим
  }
}

void showDistance() {
  long duration = ultrasonic.read(); // Отримуємо час відлуння в мікросекундах
  long duration = ultrasonic.read(); // час в мікросекундах
float speedOfSoundCmPerMicrosec = 0.0343; // Швидкість звуку при кімнатній температурі (см/мкс)
long distanceCm = duration * speedOfSoundCmPerMicrosec / 2;   // Приблизне перетворення в см
  lcd.setCursor(0, 1);
  lcd.print("Vidstan': ");
  lcd.print(distanceCm);
  lcd.print(" cm   ");
}

void controlServo() {
  long duration = ultrasonic.read(); // Отримуємо час відлуння в мікросекундах
  long duration = ultrasonic.read(); // час в мікросекундах
float speedOfSoundCmPerMicrosec = 0.0343; // Швидкість звуку при кімнатній температурі (см/мкс)
long distanceCm = duration * speedOfSoundCmPerMicrosec / 2;   // Приблизне перетворення в см
  lcd.setCursor(0, 1);
  lcd.print("Servo: ");
  if (distanceCm < 10) {
    myservo.write(90);
    lcd.print("90 grad. ");
  } else {
    myservo.write(0);
    lcd.print("0 grad.  ");
  }
}

void handleCounter() {
  if (digitalRead(buttonSelectPin) == LOW && (millis() - lastButtonSelectTime > debounceDelay)) {
    counter++;
    if (counter > 9) {
      counter = 0;
    }
    display7Segment(counter); // Викликаємо функцію для відображення на 7-сегментному індикаторі
    lcd.setCursor(0, 1);
    lcd.print("Lichyl'nyk: "); // Використовуємо латиницю
    lcd.print(counter);
    lastButtonSelectTime = millis();
    delay(200); // Затримка для уникнення багаторазового натискання
  }
  lcd.setCursor(0, 1);
  lcd.print("Lichyl'nyk: "); // Використовуємо латиницю
  lcd.print(counter);
}

void shiftOutManual(byte dataPin, byte clockPin, byte bitOrder, byte val) {
  for (int i = 0; i < 8; i++) {
    if (bitOrder == LSBFIRST)
      digitalWrite(dataPin, bitRead(val, i));
    else
      digitalWrite(dataPin, bitRead(val, 7 - i));

    digitalWrite(clockPin, HIGH);
    digitalWrite(clockPin, LOW);
  }
}

void display7Segment(int num) {
  digitalWrite(srLatchPin, LOW);
  shiftOutManual(srDataPin, srClockPin, LSBFIRST, digits[num]);
  digitalWrite(srLatchPin, HIGH);
}