#include <Servo.h>

// Создаем объекты сервоприводов
Servo servoX; // Сервопривод по оси X
Servo servoY; // Сервопривод по оси Y
Servo servoZ; // Сервопривод по оси Z

// Пины подключения сервоприводов
const int servoXPin = 9; // Пин для сервопривода X
const int servoYPin = 10; // Пин для сервопривода Y
const int servoZPin = 11; // Пин для сервопривода Z
const int pirPin = 2; // Пин для PIR датчика

// Параметры движения
const int minPosX = 0; // Минимальное положение (0 градусов)
const int maxPosX = 180; // Максимальное положение (180 градусов)
const int minPosY = 60; // Минимальное положение (0 градусов)
const int maxPosY = 150; // Максимальное положение (180 градусов)
const int minPosZ = 150; // Минимальное положение (0 градусов)
const int maxPosZ = 180; // Максимальное положение (180 градусов)
const int midPos = 90; // Среднее положение (90 градусов)
int step = 1; // Шаг движения

// Текущие и целевые положения
int currentX = midPos;
int targetX = midPos;
int currentY = midPos;
int targetY = midPos;
int currentZ = midPos;
int targetZ = midPos;

// Таймеры для движения
unsigned long moveTimerX = 0;
unsigned long moveTimerY = 0;
unsigned long moveTimerZ = 0;
unsigned long changeTimerX = 0;
unsigned long changeTimerY = 0;
unsigned long changeTimerZ = 0;
unsigned long pauseTimerX = 0; // Новый таймер для паузы X
unsigned long pauseTimerY = 0; // Новый таймер для паузы Y
unsigned long pauseTimerZ = 0; // Новый таймер для паузы Z
unsigned long emergencyTimer = 0; // Таймер для экстренной остановки

// Интервалы времени
unsigned long moveInterval = 50; // Интервал движения (мс)
const unsigned long changeInterval = 50; // Интервал смены цели (мс)
unsigned long pauseInterval = 0; // Интервал паузы
bool emergencyMode = false; // Флаг режима экстренной остановки

void setup() {
  // Инициализация сервоприводов
  servoX.attach(servoXPin);
  servoY.attach(servoYPin);
  servoZ.attach(servoZPin);
  
  // Настройка пина PIR датчика
  pinMode(pirPin, INPUT);
  
  // Устанавливаем начальное положение
  servoX.write(midPos);
  servoY.write(midPos);
  servoZ.write(midPos);
  
  // Задаем начальные таймеры
  moveTimerX = millis();
  moveTimerY = millis();
  moveTimerZ = millis();
  changeTimerX = millis();
  changeTimerY = millis();
  changeTimerZ = millis();
}

void loop() {
  step = random(1, 5);
  moveInterval = random(random(1, 50), random(50, 500));
  pauseInterval = random(1, random(1, 1000)); // Случайная пауза от 100 до 1000 мс

  // Проверка состояния PIR датчика
  if (digitalRead(pirPin) == HIGH) { // Движение обнаружено
    if (!emergencyMode) { // Если режим экстренной остановки еще не активирован
      // Устанавливаем сервоприводы в нужные позиции
      servoX.write(midPos);
      servoY.write(midPos);
      servoZ.write(0);
      
      // Сохраняем текущие позиции
      currentX = midPos;
      currentY = midPos;
      currentZ = 0;
      
      // Активируем режим экстренной остановки
      emergencyMode = true;
      emergencyTimer = millis();
    }
  } else { // Движение не обнаружено
    if (emergencyMode) { // Если режим экстренной остановки был активирован
     // Проверяем, прошло ли 15 секунд
 if (millis() - emergencyTimer >= 15000) {
 emergencyMode = false; // Выходим из режима экстренной остановки
 }
 }
 }

 // Обработка режима экстренной остановки
 if (emergencyMode) {
 return; // Блокируем движение всех осей
 }

 // Движение по оси X
 if (millis() - moveTimerX >= moveInterval) {
 if (millis() - pauseTimerX >= pauseInterval) { // Проверяем, прошла ли пауза
 moveServo(currentX, targetX, step, servoX);
 moveTimerX = millis();
 pauseTimerX = millis(); // Обновляем таймер паузы
 
 // Случайная смена цели
 if (millis() - changeTimerX >= random(1000, 360000)) {
 targetX = random(minPosX, maxPosX + 1);
 changeTimerX = millis();
 }
 }
 }
 
 // Движение по оси Y
 if (millis() - moveTimerY >= moveInterval) {
 if (millis() - pauseTimerY >= pauseInterval) { // Проверяем, прошла ли пауза
 moveServo(currentY, targetY, step, servoY);
 moveTimerY = millis();
 pauseTimerY = millis(); // Обновляем таймер паузы
 
 // Случайная смена цели
 if (millis() - changeTimerY >= random(100, 360000)) {
 targetY = random(minPosY, maxPosY + 1);
 changeTimerY = millis();
 }
 }
 }
 
 // Движение по оси Z
 if (millis() - moveTimerZ >= moveInterval) {
 if (millis() - pauseTimerZ >= pauseInterval) { // Проверяем, прошла ли пауза
 moveServo(currentZ, targetZ, step, servoZ);
 moveTimerZ = millis();
 pauseTimerZ = millis(); // Обновляем таймер паузы
 
 // Случайная смена цели
 if (millis() - changeTimerZ >= random(100, 360000)) {
 targetZ = random(minPosZ, maxPosZ + 1);
 changeTimerZ = millis();
 }
 }
 }
}

// Функция плавного перемещения сервопривода
void moveServo(int &current, int target, int step, Servo servo) {
 if (current < target) {
 current = min(current + step, target);
 } else if (current > target) {
 current = max(current - step, target);
 }
 servo.write(current);
}