#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 switchPin = 3; // Пин для движкового переключателя
const int joystickX = A0; // Пин для оси X джойстика
const int joystickY = A1; // Пин для оси Y джойстика
const int potPin = A2; // Пин для потенциометра оси Z

// Параметры самостоятельного движения
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 градусов)

// Параметры ручного движения
const int JminPosX = 0; // Минимальное положение (0 градусов)
const int JmaxPosX = 180; // Максимальное положение (180 градусов)
const int JminPosY = 0; // Минимальное положение (0 градусов)
const int JmaxPosY = 180; // Максимальное положение (180 градусов)
const int JminPosZ = 0; // Минимальное положение (0 градусов)
const int JmaxPosZ = 180; // Максимальное положение (180 градусов)

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; // Флаг режима экстренной остановки
bool manualMode = false; // Флаг ручного режима

void setup() {

  // Инициализация сервоприводов
  servoX.attach(servoXPin);
  servoY.attach(servoYPin);
  servoZ.attach(servoZPin);

  // Настройка пинов
  pinMode(pirPin, INPUT);
  pinMode(switchPin, INPUT_PULLUP);

  // Устанавливаем начальное положение
  servoX.write(midPos);
  servoY.write(midPos);
  servoZ.write(midPos);

  // Задаем начальные таймеры
  moveTimerX = millis();
  moveTimerY = millis();
  moveTimerZ = millis();
  changeTimerX = millis();
  changeTimerY = millis();
  changeTimerZ = millis();
}

void loop() {
  // Проверка состояния переключателя
  manualMode = digitalRead(switchPin) == LOW;

  if (manualMode) {
    // Ручной режим управления
    int joyX = analogRead(joystickX);
    int joyY = analogRead(joystickY);
    int potValue = analogRead(potPin);

    // Обработка сигналов джойстика
    if (joyX > 750) {
      targetX = min(currentX + step, JmaxPosX);
    } else if (joyX < 250) {
      targetX = max(currentX - step, JminPosX);
    }

    if (joyY > 750) {
      targetY = min(currentY + step, JmaxPosY);
    } else if (joyY < 250) {
      targetY = max(currentY - step, JminPosY);
    }

    // Обработка сигнала потенциометра
    int zPos = map(potValue, 0, 1023, JminPosZ, JmaxPosZ);
    targetZ = zPos;

    // Плавное перемещение осей
    moveServo(currentX, targetX, 1, servoX);
    moveServo(currentY, targetY, 1, servoY);
    moveServo(currentZ, targetZ, 1, servoZ);

  } else {
    // Автоматический режим
    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);
}