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

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

// Создаём объект сервопривода
Servo servo;

// Создаём объект матрицы
TroykaLedMatrix matrix;

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

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

// Даём понятное имя пину 5 с сервоприводом
constexpr uint8_t SERVO_PIN = 5;

// Задаём минимальный и максимальный угол поворота сервопривода
constexpr uint8_t MIN_ANGLE = 0;
constexpr uint8_t MAX_ANGLE = 180;

// Вычисляем количество градусов в каждом секторе,
// делим диапазон угла сервопривода на 8 равных секторов
constexpr uint8_t STEP_ANGLE = (MAX_ANGLE - MIN_ANGLE) / 7;

// Задаём минимальное и максимальное детектируемое расстояние дальномера в см
constexpr uint8_t MIN_DIST = 5;
constexpr uint8_t MAX_DIST = 30;

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

// Создаём константу для хранения паузы между поворотом вала сервопривода
constexpr int DELAY_SWEEP = 50;

// Создаём массив для хранения расстояния до объекта в матричном представлении
byte distMatrix[8];

void setup() {
  // Настраиваем пин с пищалкой в режим выхода
  pinMode(BUZZER_PIN, OUTPUT);
  // Инициализируем дальномер
  distSensor.attach(TRIG_PIN, ECHO_PIN, MIN_DIST, MAX_DIST);
  // Подключаем сервомотор
  servo.attach(SERVO_PIN);
  // Инициализируем матрицу
  matrix.begin();
  // Очищаем матрицу
  matrix.clear();
  // Устанавливаем ориентацию матрицы на 90 градусов
  matrix.setRotation(ROTATION_90);
}

void loop() {
  // Перебираем значения угла сервопривода от min до max
  for (int angle = MIN_ANGLE; angle <= MAX_ANGLE; angle++) {
    // Вызываем функцию обновления состояние сонара
    updateSonar(angle);
  }
  // Перебираем значения угла сервопривода от max до min
  for (int angle = MAX_ANGLE; angle >= MIN_ANGLE; angle--) {
    // Вызываем функцию обновления состояние сонара
    updateSonar(angle);
  }
}

// Функция обновления состояния сонара
void updateSonar(int angle) {
  // Отправляем текущий угол на сервопривод
  servo.write(angle);
  // Выжидаем паузу
  delay(DELAY_SWEEP);
  // Если вал мотора достиг нового сектора
  if (angle % STEP_ANGLE == 0) {
    // Вычисляем номер сектора от 0 до 7
    int colMatrix = angle / STEP_ANGLE;
    // Считываем расстояние до объекта в см
    int dist = distSensor.getDistanceCM();
    // Преобразуем диапазон значений с дальномера [MIN_DIST;MAX_DIST]
    // в графический диапазон значений для столбца матрицы от 0 до 8 точек
    distMatrix[colMatrix] = matrix.map(dist, MIN_DIST, MAX_DIST);
    // Отображаем все столбцы на матрице
    matrix.drawBitmap(distMatrix);
    // Обновляем звуковую сигнализацию
    updateAlarm(dist);
  }
}

// Функция обновления звуковой сигнализации
void updateAlarm(int dist) {
  // Если текущее расстояние до препятствия меньше тригера
  if (dist < ALARM_DIST) {
    // Включаем звуковую сигнализацию
    tone(BUZZER_PIN, 1000, 100);
  } else {
    // Выключаем звуковую сигнализацию
    noTone(BUZZER_PIN);
  }
}