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

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

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

// Даём понятное имя пину 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;

// Создаём константы для калибровки скорости левого и правого мотора
// Определите в какую сторону клонит робота
// Отклонение вправо: левый мотор быстрее правого.
// Увеличивайте константу SPEED_FACTOR_L до тех пор, пока движение не выровняется
// Отклонение влево: правый мотор быстрее левого.
// Увеличивайте константу SPEED_FACTOR_R до тех пор, пока движение не выровняется
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;

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

void setup() {
  // Настраиваем пин с пищалкой в режим выхода
  pinMode(BUZZER_PIN, OUTPUT);
  // Подключаем сервомоторы
  servoL.attach(SERVO_WHEEL_L_PIN);
  servoR.attach(SERVO_WHEEL_R_PIN);
  // Останавливаем сервомоторы
  motorsDrive(0, 0, false);
  // Включаем стартовую мелодию
  melodyStart();
}

void loop() {
  // Задаём плавно максимальную скорость вперёд
  motorsDrive(100, 100);
  // Ждём интервал задержки езды
  delay(DELAY_DRIVE);
  // Останавливаем плавно сервомоторы
  motorsDrive(0, 0);
  // Ждём интервал задержки остановки
  delay(DELAY_STOP);
}

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