// Подключаем библиотеку для работы с дальномером
#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 DISTANCE_RANGE = 100;

// Создаём константу для желаемого расстояние до объекта в см
constexpr int DISTANCE_TARGET = 30;
// Создаём константы для хранения желаемой зоны до объекта
constexpr int MIN_DISTANCE_TARGET = DISTANCE_TARGET - (0.25 * DISTANCE_TARGET);
constexpr int MAX_DISTANCE_TARGET = DISTANCE_TARGET + (0.25 * DISTANCE_TARGET);

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

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

// Прототип функции управления моторами
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);
  // Включаем стартовую мелодию
  melodyStart();
}

void loop() {
  // Считываем расстояние до объекта в см
  float distance = distSensor.getDistanceCM();
  // Задержка для стабилизации показаний с дальномера
  delay(10);
  // Проверяем расстояние до объекта
  if (distance > DISTANCE_RANGE) {
    // Если в зоне поиска робота нет объектов, стоим
    motorsDrive(0, 0);
  } else if (distance < MIN_DISTANCE_TARGET) {
    // Если робот слишком близко к цели, едем назад
    motorsDrive(-50, -50);
  } else if (distance > MIN_DISTANCE_TARGET && distance < MAX_DISTANCE_TARGET) {
    // Если робот в зоне слежения цели, стоим
    motorsDrive(0, 0);
  } else if (distance > MAX_DISTANCE_TARGET) {
    // Если робот далеко от цели, едем вперёд
    motorsDrive(50, 50);
  }
}

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