#include <Servo.h>
#include <EEPROM.h>

#define INIT_ADDR 1023  // номер резервной ячейки
#define INIT_KEY 50     // ключ первого запуска. 0-254, на выбор

// Создаем объекты сервоприводов
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 potPinX = A0; // Пин для оси X джойстика
const int potPinY = A1; // Пин для оси Y джойстика
const int potPinZ = A2; // Пин для потенциометра оси Z

// Параметры самостоятельного движения
int minPosX = 0; // Минимальное положение (0 градусов)
int maxPosX = 180; // Максимальное положение (180 градусов)
int minPosY = 60; // Минимальное положение (0 градусов)
int maxPosY = 150; // Максимальное положение (180 градусов)
int minPosZ = 150; // Минимальное положение (0 градусов)
int maxPosZ = 180; // Максимальное положение (180 градусов)
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 градусов)

// Хранение позиций для отправки в Serial
int LastPosX;
int LastPosY;
int LastPosZ;

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 emergencyPause = 15000;
unsigned long pingpong = 0; // Таймер для изменения pauseInterval

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

// Переменные для пингпонга
int maxt = 1000;
int mint = 10;

// Функция плавного перемещения сервопривода
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);
}

void setup() {

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

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

  if (EEPROM.read(INIT_ADDR) != INIT_KEY) { // первый запуск
    EEPROM.write(INIT_ADDR, INIT_KEY);      // записали ключ

    // записали стандартное значение яркости
    // в данном случае это значение переменной, объявленное выше
    EEPROM.put(0, minPosX);
    EEPROM.put(10, maxPosX);
    EEPROM.put(20, minPosY);
    EEPROM.put(30, maxPosY);
    EEPROM.put(40, minPosZ);
    EEPROM.put(50, maxPosZ);
    EEPROM.put(60, emergencyPause);
  }
  //EEPROM.get(0, LEDbright); // прочитали
  //analogWrite(LED_PIN, LEDbright);  // включили

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

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


  Serial.begin(115200);
  //Serial.setTimeout(50);
  Serial.println("Maus");
  Serial.println("Send me 0 and get help");
}

void loop() {
  pingpong = millis();

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

  if (manualMode) {
    // Ручной режим управления
    int potValueX = analogRead(potPinX);
    int potValueY = analogRead(potPinY);
    int potValueZ = analogRead(potPinZ);

    // Обработка сигналов джойстика
    int xPos = map(potValueX, 0, 1023, JminPosX, JmaxPosX);
    targetX = xPos;

    int yPos = map(potValueY, 0, 1023, JminPosY, JmaxPosY);
    targetY = yPos;

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

    //Отправка в сериал
    if (xPos != LastPosX) {
      Serial.println((String)"Pos X= " + xPos + " Y= " + yPos + " Z= " + zPos);
      //Serial.print(zPos);
      LastPosX = xPos;
    }
    if (zPos != LastPosZ) {
      Serial.println((String)"Pos X= " + xPos + " Y= " + yPos + " Z= " + zPos);
      //Serial.print(zPos);
      LastPosZ = zPos;
    }
    if (yPos != LastPosY) {
      Serial.println((String)"Pos X= " + xPos + " Y= " + yPos + " Z= " + zPos);
      //Serial.print(zPos);
      LastPosY = yPos;
    }
    // Плавное перемещение осей
    moveServo(currentX, targetX, 1, servoX);
    moveServo(currentY, targetY, 1, servoY);
    moveServo(currentZ, targetZ, 1, servoZ);

    while (Serial.available()) {
      if (Serial.availableForWrite() > 0) {
        Serial.println("Buffer its ok");
      }
      else {
        Serial.println("Buffer is full");
      }
      String s = Serial.readStringUntil(';'); // Чтение строки до символа новой строки
      int i = s.toInt();
      Serial.println("Received command: " + s); // Выведем полученную строку
      switch (i) {
        case 0:
          Serial.println("Help");
          Serial.println("0. Show help");
          Serial.println("1. Show all pos");
          Serial.println("2. Save pos");
          Serial.println("3. Get EEPROM");
          break;
        case 1:
          Serial.println((String)"Pos X= " + xPos + " Y= " + yPos + " Z= " + zPos);
          break;
        case 2:
          Serial.println("Save pos-ok");
          break;
        case 3:
          int read_MinPosX = 50;
          EEPROM.get(0, read_MinPosX);
          Serial.println(read_MinPosX);
          break;
        case 4:
          delay(100);
          Serial.println("save MinPosX");
          break;
        default:
          Serial.println("не понял");
          break;
      }
      Serial.flush();
    }
  } else {
    // Автоматический режим
    step = random(1, 5);
    moveInterval = random(random(1, 50), random(50, 500));
    if (millis() - pingpong >= 15000) {
      pingpong = millis();
      mint = mint ^ maxt;
      maxt = mint ^ maxt;
      mint = mint ^ maxt;

      Serial.println("time= " + mint);
    }
    pauseInterval = random(1, random(1, mint)); // Случайная пауза от 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();
        }
      }
    }
  }
}