// Піни для двигунів
const int motor1Pin = 8;  // Двигун 1
const int motor2Pin = 9;  // Двигун 2

// Піни для датчиків
const int sensor1Pin = A0;  // Оптрон 1 (Положення 1)
const int sensor2Pin = A1;  // Оптрон 2 (Положення 2)
const int vibrationPin = 2; // Датчик вібрації (удару)

// Значення для порогів напруги
const float thresholdVoltage = 1.8;
const int analogThreshold = thresholdVoltage * (1023.0 / 5.0);  // Порогове значення для ADC

// Часові змінні для аварійного режиму
unsigned long lastActiveTime = 0;
const unsigned long timeout = 10000;  // 10 секунд для аварійного режиму

// Прапор для перевірки, чи двигун в робочому положенні 1
bool inPosition1 = true;

void setup() {
  // Налаштовуємо пін для датчика вібрації
  pinMode(vibrationPin, INPUT_PULLUP);

  // Налаштовуємо піни для керування двигунами
  pinMode(motor1Pin, OUTPUT);
  pinMode(motor2Pin, OUTPUT);

  // Вмикаємо серійну комунікацію для дебагу
  Serial.begin(9600);

  // Ініціалізуємо двигуни у робоче положення 1 при старті
  moveToPosition1();
}

void loop() {
  // Зчитуємо значення з оптронів
  int sensor1Value = analogRead(sensor1Pin);
  int sensor2Value = analogRead(sensor2Pin);

  // Зчитуємо стан датчика вібрації
  int vibrationState = digitalRead(vibrationPin);

  // Дебаг інформація
  Serial.print("Sensor 1 (Position 1): ");
  Serial.println(sensor1Value);
  Serial.print("Sensor 2 (Position 2): ");
  Serial.println(sensor2Value);
  Serial.print("Vibration sensor: ");
  Serial.println(vibrationState);

  // Якщо двигун в положенні 1 (робоче положення) і оптрон 1 показує низьку напругу (<1.8 В)
  if (sensor1Value <= analogThreshold && inPosition1) {
    stopMotors();  // Двигун досяг положення 1, вимикаємо його
    Serial.println("Position 1 reached. Motors stopped.");
  }

  // Якщо датчик вібрації спрацьовує і ми в положенні 1, переміщуємо двигуни в положення 2
  if (vibrationState == LOW && inPosition1) {
    moveToPosition2();  // Переміщуємо двигун у положення 2

    // Як тільки двигун досягне положення 2, зупиняємо двигун
    while (analogRead(sensor2Pin) > analogThreshold) {
      // Чекаємо, поки двигун досягне положення 2
    }
    stopMotors();  // Двигун зупиняється, коли досягне положення 2
    Serial.println("Position 2 reached. Motors stopped for 5 seconds.");

    delay(5000);  // Затримка 5 секунд

    moveToPosition1();  // Повертаємо двигун у робоче положення 1
    lastActiveTime = millis();  // Оновлюємо час останньої активності
  }

  // Якщо двигун в положенні 2 і оптрон 2 показує низьку напругу (<1.8 В)
  if (sensor2Value <= analogThreshold && !inPosition1) {
    stopMotors();  // Двигун досяг положення 2, вимикаємо його
    Serial.println("Position 2 reached. Motors stopped.");
  }

  // Якщо не було активності більше 10 секунд, аварійний режим
  if (millis() - lastActiveTime > timeout) {
    stopMotors();  // Зупинка двигунів
    Serial.println("Emergency mode activated. Motors stopped.");
    while (true) {
      // Програма блокується в аварійному режимі
    }
  }

  delay(100);  // Затримка для стабільності
}

// Функція для переміщення двигунів у робоче положення 1
void moveToPosition1() {
  digitalWrite(motor1Pin, HIGH);  // Увімкнути двигун 1 для переміщення в положення 1
  digitalWrite(motor2Pin, LOW);   // Вимкнути двигун 2
  inPosition1 = true;             // Позначаємо, що ми у робочому положенні
  Serial.println("Moving to Position 1 (Working position)");
}

// Функція для переміщення двигунів у положення 2
void moveToPosition2() {
  digitalWrite(motor1Pin, LOW);   // Вимкнути двигун 1
  digitalWrite(motor2Pin, HIGH);  // Увімкнути двигун 2 для переміщення в положення 2
  inPosition1 = false;            // Позначаємо, що ми не в робочому положенні
  Serial.println("Moving to Position 2");
}

// Функція для зупинки двигунів
void stopMotors() {
  digitalWrite(motor1Pin, LOW);  // Вимикаємо обидва двигуни
  digitalWrite(motor2Pin, LOW);
  Serial.println("Motors stopped.");
}