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

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

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

//Создаем энкодеры
EncButton encX(4, 5, A0);
EncButton encY(6, 7, A1);
EncButton encZ(12, 8, A2);

// Пины подключения сервоприводов
#define servoXPin 9 // Пин для сервопривода X
#define servoYPin 10 // Пин для сервопривода Y
#define servoZPin 11 // Пин для сервопривода Z
#define pirPin 2 // Пин для PIR датчика
#define switchPin 3 // Пин для движкового переключателя
//#define potPinX A0 // Пин для оси X джойстика
//#define potPinY A1 // Пин для оси Y джойстика
//#define 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 midPosX = 90; // Среднее положение (90 градусов)
int midPosY = 90; // Среднее положение (90 градусов)
int midPosZ = 90; // Среднее положение (90 градусов)

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

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

int step = 1; // Шаг движения

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

// Таймеры для движения
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; // Таймер для экстренной остановки
int 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;

bool allowSerialInput = false; // Изначально запрещён приём данных

// Функция плавного перемещения сервопривода
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 handleManualInput() {
  allowSerialInput = true; // Разрешаем приём данных
  // Ручной режим управления
  //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 (encX.turn() & 0 < targetX < 180) {
    targetX += (encX.pressing() ? 10 : 1) * encX.dir();
  } else if (targetX < 0) targetX = targetX + 1;
  else if (targetX > 180) targetX = targetX - 1;

  if (encY.turn() & 0 < targetY < 180) {
    targetY += (encY.pressing() ? 10 : 1) * encY.dir();
  } else if (targetY < 0) targetY = targetY + 1;
  else if (targetY > 180) targetY = targetY - 1;

  if (encZ.turn() & 0 < targetZ < 180) {
    targetZ += (encZ.pressing() ? 10 : 1) * encZ.dir();
  } else if (targetZ < 0) targetZ = targetZ + 1;
  else if (targetZ > 180) targetZ = targetZ - 1;
  //if (encX.right() & targetX != 180) targetX = targetX + 1;
  //if (encY.left() & targetY != 0) targetY = targetY - 1;
  //if (encY.right() & targetY != 180) targetY = targetY + 1;
  //if (encZ.left() & targetZ != 0) targetZ = targetZ - 1;
  //if (encZ.right() & targetZ != 180) targetZ = targetZ + 1;

  //Отправка в сериал
  if (targetX != LastPosX & targetX > -1 & targetX < 181) {
    Serial.println((String)"Pos X= " + targetX + " Y= " + targetY + " Z= " + targetZ);
    //Serial.print(zPos);
    LastPosX = targetX;
  }
  if (targetY != LastPosY & targetY > -1 & targetY < 181) {
    Serial.println((String)"Pos X= " + targetX + " Y= " + targetY + " Z= " + targetZ);
    //Serial.print(zPos);
    LastPosY = targetY;
  }
  if (targetZ != LastPosZ & targetZ > -1 & targetZ < 181) {
    Serial.println((String)"Pos X= " + targetX + " Y= " + targetY + " Z= " + targetZ);
    //Serial.print(zPos);
    LastPosZ = targetZ;
  }

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

  if (allowSerialInput && Serial.available()) {
    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 current pos");
        Serial.println("2. Show saved positions");
        Serial.println("3. Show saved pause time");
        Serial.println("4. Save min pos x");
        Serial.println("5. Save max pos x");
        Serial.println("6. Save min pos y");
        Serial.println("7. Save max pos y");
        Serial.println("8. Save min pos z");
        Serial.println("9. Save max pos z");
        Serial.println("10. Set pause in ms");
        Serial.println("11. Reset default settings");
        Serial.println("12. Save Pause Position");
        break;
      case 1:
        Serial.println((String)"Pos X= " + targetX + " Y= " + targetY + " Z= " + targetY);
        break;
      case 2:
        Serial.println("In memory:");
        Serial.println((String)"minPosX=" + EEPROM[0] + " maxPosX=" + EEPROM[10]);
        Serial.println((String)"minPosY=" + EEPROM[20] + " maxPosY=" + EEPROM[30]);
        Serial.println((String)"minPosZ=" + EEPROM[40] + " maxPosZ=" + EEPROM[50]);
        Serial.println((String)"PausePos: X=" + EEPROM[60] + " Y=" + EEPROM[70] + " Z=" + EEPROM[80]);
        break;
      case 3:
        emergencyPause = loadIntFromEEPROM(90); // Загружаем целое число
        Serial.println("Current emergency pause time: " + String(emergencyPause) + " ms");
        break;
      case 4:
        minPosX = targetX;
        EEPROM.update(0, minPosX);
        Serial.println((String)"save minPosX= " + EEPROM[0]);
        break;
      case 5:
        maxPosX = targetX;
        EEPROM.update(10, maxPosX);
        Serial.println((String)"save maxPosX= " + EEPROM[10]);
        break;
      case 6:
        minPosY = targetY;
        EEPROM.update(20, minPosY);
        Serial.println((String)"save minPosY= " + EEPROM[20]);
        break;
      case 7:
        maxPosY = targetY;
        EEPROM.update(30, maxPosY);
        Serial.println((String)"save maxPosY= " + EEPROM[30]);
        break;
      case 8:
        minPosZ = targetZ;
        EEPROM.update(40, minPosZ);
        Serial.println((String)"save minPosZ= " + EEPROM[40]);
        break;
      case 9:
        maxPosZ = targetZ;
        EEPROM.update(50, maxPosZ);
        Serial.println((String)"save maxPosZ= " + EEPROM[50]);
        break;
      case 11:
        minPosX = 45; maxPosX = 135;
        minPosY = 60; maxPosY = 135;
        minPosZ = 150; maxPosZ = 180;
        EEPROM.update(0, minPosX); EEPROM.update(10, maxPosX);
        EEPROM.update(20, minPosY); EEPROM.update(30, maxPosY);
        EEPROM.update(40, minPosZ); EEPROM.update(50, maxPosZ);
        Serial.println("Reset default settings- OK");
        break;
      case 12:
        midPosX = targetX;
        midPosY = targetY;
        midPosZ = targetZ;
        EEPROM.update(60, midPosX); EEPROM.update(70, midPosY); EEPROM.update(80, midPosZ);
        Serial.println((String)"save midPosX=" + EEPROM[60] + " midPosY=" + EEPROM[70] + " midPosZ=" + EEPROM[80]);
        break;
      case 10:
        Serial.println("Enter the value in milliseconds or type 'Stop' to exit");

        while (!Serial.available()); // Ждём поступления данных

        String input = Serial.readStringUntil('\n');

        if (input.equalsIgnoreCase("stop")) {
          Serial.println("Exiting set pause mode");
          break;
        }

        int newEmergencyPause = input.toInt();

        if (newEmergencyPause > 0 & newEmergencyPause <= 90000) {
          saveIntToEEPROM(90, newEmergencyPause); // Сохраняем целое число
          emergencyPause = newEmergencyPause;
          Serial.println("New emergency pause time: " + String(newEmergencyPause) + " ms");
        } else {
          Serial.println("Invalid input. Please enter a valid number of milliseconds between 1 and 30000");
        }
        break;
        //default:
        //  Serial.println("не понял");
        //  break;
    }

  }
  allowSerialInput = false; // Возвращаем режим запрета приёма данных
}

void autoControl() {
  // Чистка буфера при каждом входе в автоматический режим
  while (Serial.available() > 0) {
    Serial.read(); // Прочитаем и отбросим все данные
  }
  // Автоматический режим
  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(midPosX);
      servoY.write(midPosY);
      servoZ.write(midPosZ);

      // Сохраняем текущие позиции
      currentX = midPosX;
      currentY = midPosY;
      currentZ = midPosZ;

      // Активируем режим экстренной остановки
      emergencyMode = true;
      emergencyTimer = millis();
    }
  } else { // Движение не обнаружено
    if (emergencyMode) { // Если режим экстренной остановки был активирован
      // Проверяем, прошло ли 15 секунд
      if (millis() - emergencyTimer >= emergencyPause) {
        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 saveIntToEEPROM(int address, int value) {
  byte lowByte = lowByte(value); // Младший байт
  byte highByte = highByte(value); // Старший байт
  EEPROM.update(address, lowByte);
  EEPROM.update(address + 1, highByte);
}

int loadIntFromEEPROM(int address) {
  byte lowByte = EEPROM.read(address);
  byte highByte = EEPROM.read(address + 1);
  return word(highByte, lowByte); // Собираем обратно целое число
}

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, midPosX);
    EEPROM.put(70, midPosY);
    EEPROM.put(80, midPosZ);
    EEPROM.put(90, emergencyPause);
  }
  //EEPROM.get(0, LEDbright); // прочитали
  //analogWrite(LED_PIN, LEDbright);  // включили

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

  // Задаем начальные таймеры
  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");
  delay(500);
}

void loop() {
  encX.tick();
  encY.tick();
  encZ.tick();

  pingpong = millis();

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

  if (manualMode) {
    handleManualInput();
  } else {
    autoControl();
  }
}