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

LiquidCrystal_I2C lcd(0x27, 16, 2); // Адрес 0x3F для I2C экрана 16x2
Servo servos[6];
int servoAngles[6] = {90, 90, 90, 90, 90, 90}; // Изначальные углы серводвигателей
int currentServo = 0;
int currentPotPin = A3; // Порт для текущего потенциометра (подключенного к A3)
int buttons[] = {1, 2, 4, 7, 8, 12, 13}; // Порты кнопок, соответствующие серводвигателям
int potPins[] = {A2, A2, A2, A2, A2, A2}; // Порты для потенциометров, соответствующие серводвигателям
int servoPorts[] = {3, 5, 6, 9, 10, 11}; // Порты серводвигателей
int switchLeftPin = 7; // Порт для левого положения переключателя
int switchRightPin = 4; // Порт для правого положения переключателя
bool allowPotChange = true; // Разрешение на изменение потенциометра A2

void setup() {
  lcd.init();                      // Инициализация экрана
  lcd.backlight();                 // Включение подсветки экрана
  lcd.setCursor(0, 0);
  lcd.print("Servo angles:");

  for (int i = 0; i < 6; i++) {
    servos[i].attach(servoPorts[i]); // Подключение серводвигателей к портам 3, 5, 6, 9, 10, 11
    servos[i].write(servoAngles[i]);
  }

  for (int i = 0; i < 7; i++) {
    pinMode(buttons[i], INPUT_PULLUP); // Установка кнопок в режим INPUT_PULLUP
  }

  pinMode(switchLeftPin, INPUT_PULLUP); // Установка левого положения переключателя в режим INPUT_PULLUP
  pinMode(switchRightPin, INPUT_PULLUP); // Установка правого положения переключателя в режим INPUT_PULLUP
}

void loop() {
  int potValue1 = analogRead(A3); // Считываем значение первого потенциометра (подключенного к A3)
  int potValue2 = analogRead(A2); // Считываем значение второго потенциометра (подключенного к A2)

  // Определяем текущий серводвигатель в зависимости от числа на экране
  int newServo = map(potValue1, 0, 1023, 0, 5);

  // Определяем состояние переключателя
  int switchStateLeft = digitalRead(switchLeftPin);
  int switchStateRight = digitalRead(switchRightPin);

  // Проверяем положение переключателя и устанавливаем флаг allowPotChange соответствующим образом
  if (switchStateLeft == LOW) {
    allowPotChange = false; // Переключатель на левой стороне - блокируем изменение потенциометра A2
  } else if (switchStateRight == LOW) {
    allowPotChange = true; // Переключатель на правой стороне - разрешаем изменение потенциометра A2
  }

  if (newServo != currentServo) {
    currentServo = newServo;
    currentPotPin = potPins[currentServo]; // Устанавливаем текущий потенциометр для текущего серводвигателя
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Servo ");
    lcd.print(currentServo + 1);
    lcd.setCursor(0, 1);
    lcd.print("Angle: ");
    // Форматируем угол с ведущими нулями для двухзначных чисел
    lcd.print(servoAngles[currentServo] < 100 ? (servoAngles[currentServo] < 10 ? "00" : "0") : "");
    lcd.print(servoAngles[currentServo]);
  }

  for (int i = 0; i < 7; i++) {
    if (buttons[i] == currentServo + 1) {
      int buttonState = digitalRead(buttons[i]);
      if (buttonState == LOW) {
        servoAngles[currentServo] += 5; // Увеличиваем угол поворота на 5 градусов
        if (servoAngles[currentServo] > 180) {
          servoAngles[currentServo] = 0; // Ограничиваем угол до 180 градусов
        }
        servos[currentServo].write(servoAngles[currentServo]);
        lcd.setCursor(7, 1);
        // Форматируем угол с ведущими нулями для двухзначных чисел
        lcd.print(servoAngles[currentServo] < 100 ? (servoAngles[currentServo] < 10 ? "00" : "0") : "");
        lcd.print(servoAngles[currentServo]);
        delay(100); // Задержка для стабильности
      }
    }
  }

  // Управление углом с использованием потенциометра A2, если разрешено
  if (currentPotPin == A2 && allowPotChange) {
    int potValue = analogRead(A2); // Считываем значение потенциометра A2
    int servoIndex = currentServo; // Определяем серводвигатель, соответствующий текущему положению
    int mappedValue = map(potValue, 0, 1023, 0, 180); // Маппим значение потенциометра в угол поворота (0-180 градусов)
    servos[servoIndex].write(mappedValue); // Устанавливаем угол поворота для соответствующего серводвигателя
    servoAngles[servoIndex] = mappedValue; // Обновляем угол в массиве
    lcd.setCursor(7, 1);
    // Форматируем угол с ведущими нулями для двухзначных чисел
    lcd.print(mappedValue < 100 ? (mappedValue < 10 ? "00" : "0") : "");
    lcd.print(mappedValue);
  }

  
}