// Подключаем библиотеку для работы с дальномером
#include <EasyUltrasonic.h>

// Подключаем библиотеку для работы с сервомоторами
#include <Servo.h>

// Даём понятное имя пину 3 с пищалкой
constexpr uint8_t BUZZER_PIN = 3;

// Даём понятные имена пинам 12 и 11 с дальномером
constexpr uint8_t TRIG_PIN = 12;
constexpr uint8_t ECHO_PIN = 11;

// Даём понятное имя пину 7 с левым сервомотором
constexpr uint8_t SERVO_WHEEL_L_PIN = 7;

// Даём понятное имя пину 10 с правым сервомотором
constexpr uint8_t SERVO_WHEEL_R_PIN = 10;

// Задаём максимально доступную скорость сервомоторов
// в сырых значениях по часовой и против часовой стрелки
constexpr int SPEED_RAW_MAX_CW = 30;
constexpr int SPEED_RAW_MAX_CCW = 150;

// Вычисляем показатель остановки сервомоторов в сырых значениях
constexpr int SPEED_RAW_STOP = (SPEED_RAW_MAX_CW + SPEED_RAW_MAX_CCW) / 2;

// Создаём константы для калибровки скорости левого и правого мотора
constexpr int SPEED_FACTOR_L = 0;
constexpr int SPEED_FACTOR_R = 30;

// Создаём константу задержки езды
constexpr int DELAY_DRIVE = 3000;
// Создаём константу задержки остановки
constexpr int DELAY_STOP = 1000;
// Создаём константу задержки плавной остановки
constexpr int DELAY_SMOOTH = 30;

// Создаём константу для хранения базовой частоты
constexpr int FREQUENCY = 2000;

// Создаём константы для хранения минимальной и максимальной частоты
constexpr int MIN_FREQUENCY = FREQUENCY - (0.25 * FREQUENCY);
constexpr int MAX_FREQUENCY = FREQUENCY + (0.25 * FREQUENCY);

// Создаём константу для хранения тригера сигнализации в см
constexpr int DISTANCE_DETOUR = 20;

// Создаём объект для работы с дальномером
EasyUltrasonic distSensor;

// Создаём объект для работы с сервомоторами
Servo servoL;
Servo servoR;

// Создаём перечисление состояний робота с соответствующей переменной
enum {
  ROBOT_DRIVE, // Робот едет вперёд
  ROBOT_DETECT_DETOUR, // Робот зафиксировал препятствие
} robotState;

// Создаём перечисление направления движения робота с соответствующей переменной
enum DirectionDetours {
  LEFT,
  RIGHT,
} direction;

// Прототип функции управления моторами
void motorsDrive(int speedL, int speedR, bool smooth = true);

void setup() {
  // Настраиваем пин с пищалкой в режим выхода
  pinMode(BUZZER_PIN, OUTPUT);
  // Подключаем дальномер
  distSensor.attach(TRIG_PIN, ECHO_PIN);
  // Подключаем сервомоторы
  servoL.attach(SERVO_WHEEL_L_PIN);
  servoR.attach(SERVO_WHEEL_R_PIN);
  // Останавливаем сервомоторы
  motorsDrive(0, 0, false);
  // Передаём случайное число с пина A1
  // для последующей генерации случайных чисел
  randomSeed(analogRead(A1));
  // Включаем стартовую мелодию
  melodyStart();
  // Устанавливаем режим «Робот едет вперёд»
  robotState = ROBOT_DRIVE;
}

void loop() {
  // Если установлен режим «Робот едет вперёд»
  if (robotState == ROBOT_DRIVE) {
    // Переходим в функцию обработки режима «Робот едет вперёд»
    handleRobotDrive();
  }
  // Если установлен режим «Робот зафиксировал препятствие»
  if (robotState == ROBOT_DETECT_DETOUR) {
    // Переходим в функцию обработки режима «Робот зафиксировал препятствие»
    handleRobotDetectDetour();
  }
}

// Функция обработки режима «Робот едет вперёд»
void handleRobotDrive() {
  // Задаём максимальную скорость вперёд
  motorsDrive(100, 100, false);
  // Считываем расстояние до объекта в см
  float distance = distSensor.getDistanceCM();
  // Задержка для стабилизации показаний с дальномера
  delay(10);
  // Если текущее расстояние до препятствия меньше тригера
  if (distance < DISTANCE_DETOUR) {
    // Устанавливаем режим «Робот зафиксировал препятствие»
    robotState = ROBOT_DETECT_DETOUR;
  }
}

// Функция обработки режима «Робот зафиксировал препятствие»
void handleRobotDetectDetour() {
  // Останавливаем плавно сервомоторы
  motorsDrive(0, 0);
  // Запускаем мелодию R2-D2
  melodyR2D2();
  // Ждём 1 сек
  delay(1000);
  // Объезжаем препятствие слева 500 мс
  motorsDetourBarrier(LEFT, 500);
  // Устанавливаем режим «Робот едет вперёд»
  robotState = ROBOT_DRIVE;
}

// Функция объезда препятствие
void motorsDetourBarrier(DirectionDetours direction, int delayDrive) {
  // Выбираем направления заднего хода для разворота
  if (direction == LEFT) {
    motorsDrive(-50, 0, false);
  } else if (direction == RIGHT) {
    motorsDrive(0, -50, false);
  }
  // Ждём интервал задержки езды
  delay(delayDrive);
  // Останавливаем сервомоторы с плавным торможением
  motorsDrive(0, 0);
  // Ждём интервал задержки остановки
  delay(1000);
}

// Функция управления сервомоторами
void motorsDrive(int speedL, int speedR, bool smooth) {
  // Создаём переменные для хранения текущей скорости
  // левого и правого мотора в сырых значениях
  int speedRawL = 0;
  int speedRawR = 0;
  // Создаём переменные для хранения обновлённой скорости
  // левого и правого мотора в сырых значениях
  int newSpeedRawL = 0;
  int newSpeedRawR = 0;

  // Создаём переменные для хранения калибровочных коэффициентов скорости
  // левого и правого мотора
  int speedFactorL = 0;
  int speedFactorR = 0;

  // Вычисляем калибровочные коэффициенты для моторов
  speedFactorL = (SPEED_FACTOR_L * speedL) / 100;
  speedFactorR = (SPEED_FACTOR_R * speedR) / 100;

  // Получаем текущие показания моторов в сырых значениях
  speedRawL = servoL.read();
  speedRawR = servoR.read();

  // Преобразуем показания скорости левого мотора
  if (speedL == 0) {
    newSpeedRawL = SPEED_RAW_STOP;
  } else if (speedL > 0) {
    speedL -= speedFactorL;
    newSpeedRawL = map(abs(speedL), 0, 100, SPEED_RAW_STOP, SPEED_RAW_MAX_CCW);
  } else if (speedL < 0) {
    speedL += speedFactorL;
    newSpeedRawL = map(abs(speedL), 0, 100, SPEED_RAW_STOP, SPEED_RAW_MAX_CW);
  }

  // Преобразуем показания скорости правого мотора
  if (speedR == 0) {
    newSpeedRawR = SPEED_RAW_STOP;
  } else if (speedR > 0) {
    speedR -= speedFactorR;
    newSpeedRawR = map(abs(speedR), 0, 100, SPEED_RAW_STOP, SPEED_RAW_MAX_CW);
  } else if (speedR < 0) {
    speedR += speedFactorR;
    newSpeedRawR = map(abs(speedR), 0, 100, SPEED_RAW_STOP, SPEED_RAW_MAX_CCW);
  }

  // Проверяем состояние флага: плавное или мгновенное изменение скорости
  if (smooth) {
    // Плавное изменение скорости
    int steps = 10;
    for (int i = 0; i <= steps; i++) {
      int newSpeedRawSmoothL = map(i, 0, steps, speedRawL, newSpeedRawL);
      int newSpeedRawSmoothR = map(i, 0, steps, speedRawR, newSpeedRawR);
      servoL.write(newSpeedRawSmoothL);
      servoR.write(newSpeedRawSmoothR);
      delay(DELAY_SMOOTH);
    }
  } else {
    // Мгновенное изменение скорости
    servoL.write(newSpeedRawL);
    servoR.write(newSpeedRawR);
  }
}

// Функция стартовой мелодии
void melodyStart() {
  tone(BUZZER_PIN, 300, 500);
  delay(1000);
  tone(BUZZER_PIN, 400, 500);
  delay(1000);
  tone(BUZZER_PIN, 500, 500);
  delay(1000);
  tone(BUZZER_PIN, 700, 300);
  delay(1000);
}

// Функция мелодии R2-D2
void melodyR2D2() {
  // Вызываем функцию генератора последовательных длинных семплов
  toneLongSamples();
  // Вызываем функцию генератора непоследовательных коротких семплов
  toneShortSamples();
}

// Функция генерации последовательных длинных семплов
void toneLongSamples() {
  // Генерируем случайное количество семплов от 1 до 6
  int countSamples = random(1, 7);
  // Перебираем нумерацию семплов в цикле
  for (int i = 0; i < countSamples; i++) {
    // Генерируем случайное число: true или false
    bool typeSamples = random(0, 2);
    // Если тип семпла true
    if (typeSamples) {
      // Вызываем функцию генерации тона toneSlowDownFastUp:
      // Сначала медленный убывающий тон со случайными задержками
      // Затем быстрый возрастающий тон со случайными задержками
      toneSlowDownFastUp();
    } else {
      // Если тип семпла false
      // Вызываем функцию генерации тона toneSlowUpFastDown:
      // Сначала медленный возрастающий тон со случайными задержками
      // Затем быстрый убывающий тон со случайными задержками
      toneSlowUpFastDown();
    }
  }
}

// Функция генерации непоследовательных коротких семплов
void toneShortSamples() {
  // Генерируем случайное количество семплов от 3 до 9
  int countSamples = random(3, 10);
  // Генерируем случайную частоту на базе FREQUENCY
  int frequency = random(MIN_FREQUENCY, MAX_FREQUENCY);
  for (int i = 0; i <= countSamples; i++) {
    // Генерируем случайный интервал на базе FREQUENCY
    int range = random(-FREQUENCY, FREQUENCY);
    tone(BUZZER_PIN, frequency + range);
    // Выполняем случайную задержку от 70 до 170 мс
    delay(random(70, 170));
    // Выключаем звук
    noTone(BUZZER_PIN);
    // Выполняем случайную задержку от 0 до 30 мс
    delay(random(0, 30));
  }
}

// Функция генерации звуковой последовательности
// Сначала медленный убывающий тон со случайными задержками
// Затем быстрый возрастающий тон со случайными задержками
void toneSlowDownFastUp() {
  // Генерируем случайную частоту на базе FREQUENCY
  int frequency = random(MIN_FREQUENCY, MAX_FREQUENCY);
  // Генерируем медленный понижающий тон со случайными задержками
  for (int range = 0; range <= random(100, 1000); range++) {
    tone(BUZZER_PIN, (range * 2));
    delayMicroseconds(random(0, 1000));
  }
  // Генерируем быстрый повышающий тон со случайными задержками
  for (int range = 0; range <= random(100, 1000); range++) {
    tone(BUZZER_PIN, frequency + (range * 10));
    delayMicroseconds(random(0, 1000));
  }
}

// Функция генерации звуковой последовательности
// Сначала медленный возрастающий тон со случайными задержками
// Затем быстрый убывающий тон со случайными задержками
void toneSlowUpFastDown() {
  // Генерируем случайную частоту на базе FREQUENCY
  int frequency = random(MIN_FREQUENCY, MAX_FREQUENCY);
  // Генерируем медленный возрастающий тон со случайными задержками
  for (int range = 0; range <= random(100, 1000); range++) {
    tone(BUZZER_PIN, (range * 2));
    delayMicroseconds(random(0, 1000));
  }
  // Генерируем быстрый понижающий тон со случайными задержками
  for (int range = 0; range <= random(100, 1000); range++) {
    tone(BUZZER_PIN, (range * 10));
    delayMicroseconds(random(0, 1000));
  }
}