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

// ====================== Піни ======================
// Кнопки
#define BUTTON_UP       2   // "UP"    -> D2 (із GND і INPUT_PULLUP)
#define BUTTON_DOWN     4   // "DOWN"  -> D4 (із GND і INPUT_PULLUP)
#define BUTTON_SELECT   3   // "SELECT (SET)" -> D3 (із GND і INPUT_PULLUP)

// Ультразвук
#define TRIG_PIN       10   // HC-SR04 Trigger
#define ECHO_PIN       11   // HC-SR04 Echo

// Серво
#define SERVO_PIN       9   // Сигналовий вивід серво

// RGB-світлодіод (спільний катод — COM -> GND)
// Якщо у вас спільний анод -> COM -> 5V та інвертуйте логіку вмикання
#define RED_PIN         5
#define GREEN_PIN       6
#define BLUE_PIN        7

// ====================== OLED ======================
#define SCREEN_WIDTH   128
#define SCREEN_HEIGHT   64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// ====================== Серво ======================
Servo myServo;
int servoAngle = 90;  // Початковий кут сервоприводу (0..180)

// ====================== Логіка меню ======================
int  menuIndex = 0;        // Поточний пункт
const int menuItems = 3;   // Кількість пунктів

// RGB LED (стан кожного кольору - для демонстрації беремо один стан за раз)
bool redOn   = false;
bool greenOn = false;
bool blueOn  = false;
// ----------------------------------------------------
//  ІНІЦІАЛІЗАЦІЯ OLED
// ----------------------------------------------------
void initDisplay() {
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    // Якщо не знайдено дисплей, зависнути
    for (;;) {}
  }
  display.clearDisplay();
  display.display();
}

// ----------------------------------------------------
//  ВИМІРЮВАННЯ ВІДСТАНІ HC-SR04
// ----------------------------------------------------
float getDistanceCm() {
  // 1. Коротко опускаємо TRIG
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);

  // 2. Подати імпульс ~10 мкс
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // 3. Чекаємо, поки ECHO стане HIGH, і вимірюємо тривалість
  long duration = pulseIn(ECHO_PIN, HIGH);

  // 4. Обчислюємо відстань (см)
  float distance = duration * 0.0343 / 2.0;  
  return distance;
}

// ----------------------------------------------------
//  ОНОВИТИ СТАН RGB LED
//  (для спільного катода: HIGH = увімкнено, LOW = вимкнено)
// ----------------------------------------------------
void updateRGB() {
  digitalWrite(RED_PIN,   redOn   ? LOW : HIGH);
  digitalWrite(GREEN_PIN, greenOn ? LOW : HIGH);
  digitalWrite(BLUE_PIN,  blueOn  ? LOW : HIGH);
}

// ----------------------------------------------------
//  ВІДОБРАЖЕННЯ ГОЛОВНОГО МЕНЮ НА OLED
// ----------------------------------------------------
void showMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);

  // Пункти: 0) RGB LED, 1) Ultrasonic, 2) Servo
  for (int i = 0; i < menuItems; i++) {
    if (i == menuIndex) {
      display.setCursor(0, i * 10);
      display.print("> ");
    } else {
      display.setCursor(0, i * 10);
      display.print("  ");
    }

    switch (i) {
      case 0: display.print("RGB LED");       break;
      case 1: display.print("Ultrasonic");    break;
      case 2: display.print("Servo");         break;
    }
  }

  display.display();
}

// ----------------------------------------------------
//  ОБРОБКА ВИБОРУ ПУНКТУ МЕНЮ
// ----------------------------------------------------
void handleMenuSelect(int index) {
  switch (index) {
    case 0:
      // RGB LED
      toggleRGB();
      break;

    case 1:
      // Ультразвук
      showUltrasonicDistance();
      break;

    case 2:
      // Серво
      servoMenu();
      break;
  }
}

// ----------------------------------------------------
//  ПЕРЕМИКАННЯ КОЛЬОРІВ RGB (циклічно: 0=off,1=R,2=G,3=B)
// ----------------------------------------------------
void toggleRGB() {
  static int colorState = 0;
  colorState = (colorState + 1) % 4;

  switch (colorState) {
  case 0:
    redOn   = false;
    greenOn = false;
    blueOn  = false;
    break;
  case 1:
    redOn   = true;  // червоний
    greenOn = false;
    blueOn  = false;
    break;
  case 2:
    redOn   = false;
    greenOn = true;  // зелений
    blueOn  = false;
    break;
  case 3:
    redOn   = false;
    greenOn = false;
    blueOn  = true;  // синій
    break;
}
updateRGB();


  // Показати, який колір вмикнувся
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.print("RGB state: ");
  display.print(colorState);
  display.display();

  Serial.print("RGB LED state -> ");
  Serial.println(colorState);

  delay(1000);  // Невелика пауза, щоб побачити на екрані
}

// ----------------------------------------------------
//  УЛЬТРАЗВУК: ВИВЕСТИ ВІДСТАНЬ НА OLED
// ----------------------------------------------------
void showUltrasonicDistance() {
  float dist = getDistanceCm();

  Serial.print("Ultrasonic distance = ");
  Serial.print(dist);
  Serial.println(" cm");

  // Вивід на OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.print("Distance: ");
  display.print(dist);
  display.print(" cm");
  display.display();

  delay(2000);  // Щоб встигли побачити результат
}

// ----------------------------------------------------
//  ПІДМЕНЮ КЕРУВАННЯ СЕРВО
// ----------------------------------------------------
void servoMenu() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.setTextSize(1);
  display.print("Servo control:\n");
  display.print("UP -> +10 deg\nDOWN -> -10 deg\nSELECT -> exit");
  display.display();

  while (true) {
    // Якщо натиснули UP -> +10°
    if (digitalRead(BUTTON_UP) == LOW) {
      servoAngle += 10;
      if (servoAngle > 180) servoAngle = 180;
      myServo.write(servoAngle);
      Serial.print("Servo angle = ");
      Serial.println(servoAngle);
      delay(200); // антидребезг
    }
    // Якщо натиснули DOWN -> -10°
    if (digitalRead(BUTTON_DOWN) == LOW) {
      servoAngle -= 10;
      if (servoAngle < 0) servoAngle = 0;
      myServo.write(servoAngle);
      Serial.print("Servo angle = ");
      Serial.println(servoAngle);
      delay(200);
    }
    // Якщо SELECT -> вихід із підменю
    if (digitalRead(BUTTON_SELECT) == LOW) {
      Serial.println("Exit servo menu");
      delay(300);
      break;
    }
  }
}

// ====================== SETUP ======================
void setup() {
  // 1. Серійний монітор
  Serial.begin(9600);
  Serial.println("Start...");

  // 2. Налаштування кнопок (використовуємо внутрішній pull-up)
  pinMode(BUTTON_UP,     INPUT_PULLUP);
  pinMode(BUTTON_DOWN,   INPUT_PULLUP);
  pinMode(BUTTON_SELECT, INPUT_PULLUP);

  // 3. Піни для RGB (спільний катод)
  pinMode(RED_PIN,   OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(BLUE_PIN,  OUTPUT);

  // 4. HC-SR04
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // 5. Серво
  myServo.attach(SERVO_PIN);
  myServo.write(servoAngle); // Початковий кут (90°)

  // 6. OLED
  Wire.begin();
  initDisplay();

  // Показати головне меню відразу
  showMenu();
}

// ====================== LOOP ======================
void loop() {
  // Перевірка кнопки SELECT
  if (digitalRead(BUTTON_SELECT) == LOW) {
  Serial.println("SELECT pressed!");
  // Почекати, поки відпустять кнопку
  while (digitalRead(BUTTON_SELECT) == LOW) {
    delay(10);
  }
  handleMenuSelect(menuIndex);
  showMenu();
  delay(300);
}

  // Перевірка кнопки UP
  if (digitalRead(BUTTON_UP) == LOW) {
    Serial.println("UP pressed!");
    menuIndex--;
    if (menuIndex < 0) menuIndex = menuItems - 1;
    showMenu();
    delay(200);
  }

  // Перевірка кнопки DOWN
  if (digitalRead(BUTTON_DOWN) == LOW) {
    Serial.println("DOWN pressed!");
    menuIndex++;
    if (menuIndex >= menuItems) menuIndex = 0;
    showMenu();
    delay(200);
  }
}