// Пин подключения потенциометра компаса
#define COMPASS_POT_PIN 34
// Пин подключения потенциометра курса
#define COURSE_POT_PIN 35

// Диапазон азимутов компаса
#define COMPASS_MIN -180
#define COMPASS_MAX 180

// Диапазон курса яхты
#define COURSE_MIN 0
#define COURSE_MAX 360

// Калибровочные значения (зависит от конкретных потенциометров)
#define POT_MIN 0
#define POT_MAX 4095

// Переменные для хранения значений
int compassAzimuth = 0;
int yachtCourse = 0;

void setup() {
  // Инициализация последовательного порта
  Serial.begin(115200);
  
  // Настройка пинов для потенциометров
  pinMode(COMPASS_POT_PIN, INPUT);
  pinMode(COURSE_POT_PIN, INPUT);
  
  Serial.println("ESP32 Compass and Yacht Course Emulator");
  Serial.println("======================================");
  delay(1000);
}

void loop() {
  // Чтение значений с потенциометров
  int compassRaw = analogRead(COMPASS_POT_PIN);
  int courseRaw = analogRead(COURSE_POT_PIN);
  
  // Преобразование сырых значений в углы
  compassAzimuth = map(compassRaw, POT_MIN, POT_MAX, COMPASS_MIN, COMPASS_MAX);
  yachtCourse = map(courseRaw, POT_MIN, POT_MAX, COURSE_MIN, COURSE_MAX);
  
  // Нормализация углов
  compassAzimuth = normalizeAngle(compassAzimuth, COMPASS_MIN, COMPASS_MAX);
  yachtCourse = normalizeAngle(yachtCourse, COURSE_MIN, COURSE_MAX);
  
  // Вычисление относительного курса
  int relativeCourse = calculateRelativeCourse(compassAzimuth, yachtCourse);
  
  // Вывод информации в Serial Monitor
  displayInfo(compassRaw, courseRaw, compassAzimuth, yachtCourse, relativeCourse);
  
  delay(500); // Задержка для стабильности чтения
}

// Функция для нормализации угла в заданном диапазоне
int normalizeAngle(int angle, int minAngle, int maxAngle) {
  int range = maxAngle - minAngle + 1;
  
  if (angle < minAngle) {
    angle += range * ((minAngle - angle) / range + 1);
  } else if (angle > maxAngle) {
    angle -= range * ((angle - maxAngle) / range + 1);
  }
  
  return angle;
}

// Функция для вычисления относительного курса
int calculateRelativeCourse(int compass, int course) {
  int relative = course - compass;
  return normalizeAngle(relative, -180, 179);
}

// Функция для отображения информации
void displayInfo(int compassRaw, int courseRaw, int compass, int course, int relative) {
  Serial.println("=== COMPASS & COURSE INFO ===");
  Serial.print("Compass Pot Raw: ");
  Serial.print(compassRaw);
  Serial.print(" | Azimuth: ");
  Serial.print(compass);
  Serial.println("°");
  
  Serial.print("Course Pot Raw: ");
  Serial.print(courseRaw);
  Serial.print(" | Course: ");
  Serial.print(course);
  Serial.println("°");
  
  Serial.print("Relative Course: ");
  Serial.print(relative);
  Serial.println("°");
  
  // Определение направления
  String direction = getDirection(relative);
  Serial.print("Direction: ");
  Serial.println(direction);
  
  Serial.println("-----------------------------");
  Serial.println();
}

// Функция для получения текстового направления
String getDirection(int angle) {
  if (angle >= -22 && angle <= 22) return "Прямо";
  else if (angle > 22 && angle <= 67) return "Право вперед";
  else if (angle > 67 && angle <= 112) return "Право";
  else if (angle > 112 && angle <= 157) return "Право назад";
  else if (angle > 157 || angle < -157) return "Назад";
  else if (angle >= -157 && angle <= -112) return "Лево назад";
  else if (angle >= -112 && angle <= -67) return "Лево";
  else if (angle >= -67 && angle <= -22) return "Лево вперед";
  
  return "Неопределено";
}

// Дополнительная функция для плавного чтения (опционально)
int smoothAnalogRead(int pin, int samples = 10) {
  long sum = 0;
  for (int i = 0; i < samples; i++) {
    sum += analogRead(pin);
    delay(2);
  }
  return sum / samples;
}


/*#include <Wire.h>
#include <QMC5883LCompass.h>

// Состояния галсов
enum TackState {
  PORT_TACK,      // Ветер с левого борта
  STARBOARD_TACK, // Ветер с правого борта
  TACKING,        // В процессе смены галса
  ON_COURSE       // Прямой курс (попутный ветер)
};

// Глобальные переменные
TackState current_tack = PORT_TACK;
unsigned long tack_start_time = 0;
float overall_target_course = 350.0; // Истинный целевой курс
float current_tack_course = 0.0;     // Текущий курс галса

// Константы
const float TACK_ANGLE = 45.0;       // Угол галса относительно ветра
const float MIN_TACK_DURATION = 60.0; // Минимальное время галса (сек)
const float TACK_MANEUVER_TIME = 30.0; // Время смены галса (сек)

void setup() {
  Serial.begin(9600);
  tack_start_time = millis();
  determineInitialTack(); // Определяем начальный галс
}

// Определение начального галса
void determineInitialTack() {
  float wind_direction = getWindDirection(); // 0-360°
  float boat_heading = compass.getAzimuth(); // -180..180°
  
  // Нормализуем курс яхты в 0-360°
  float normalized_heading = boat_heading < 0 ? boat_heading + 360 : boat_heading;
  
  // Определяем относительное направление ветра
  float relative_wind = calculateCourseError(normalized_heading, wind_direction);
  
  if (relative_wind > 0) {
    current_tack = PORT_TACK;     // Ветер слева
    Serial.println("Начальный галс: Левый борт (ветер слева)");
  } else {
    current_tack = STARBOARD_TACK; // Ветер справа
    Serial.println("Начальный галс: Правый борт (ветер справа)");
  }
  
  // Устанавливаем курс для галса
  current_tack_course = calculateTackCourse(wind_direction);
}

// Расчет курса для текущего галса
float calculateTackCourse(float wind_direction) {
  float tack_course;
  
  switch(current_tack) {
    case PORT_TACK:
      // Ветер слева, идем правым галсом
      tack_course = wind_direction + TACK_ANGLE;
      break;
      
    case STARBOARD_TACK:
      // Ветер справа, идем левым галсом  
      tack_course = wind_direction - TACK_ANGLE;
      break;
      
    case ON_COURSE:
      // Прямой курс - используем общий целевой курс
      tack_course = overall_target_course;
      break;
      
    case TACKING:
      // Во время маневра курс вычисляется отдельно
      tack_course = calculateTackingCourse(wind_direction);
      break;
  }
  
  // Нормализация курса
  if (tack_course >= 360.0) tack_course -= 360.0;
  if (tack_course < 0.0) tack_course += 360.0;
  
  return tack_course;
}

// Процесс смены галса
void performTack() {
  Serial.println("🚢 НАЧАЛО СМЕНЫ ГАЛСА!");
  current_tack = TACKING;
  tack_start_time = millis();
  
  // Плавный поворот через фордевинд или оверштаг
  executeTackManeuver();
}

// Выполнение маневра смены галса
void executeTackManeuver() {
  float wind_direction = getWindDirection();
  unsigned long tack_phase_start = millis();
  
  // Фаза 1: Поворот на 90° через ветер
  float phase1_target = current_tack_course + 90.0;
  if (phase1_target >= 360.0) phase1_target -= 360.0;
  
  Serial.println("Фаза 1: Поворот через ветер");
  steerToCourse(phase1_target);
  
  // Ждем завершения поворота
  while (!isCourseReached(phase1_target, 5.0)) {
    delay(100);
  }
  
  // Фаза 2: Выход на новый галс
  TackState new_tack = (current_tack == PORT_TACK) ? STARBOARD_TACK : PORT_TACK;
  float phase2_target = calculateTackCourseForState(wind_direction, new_tack);
  
  Serial.println("Фаза 2: Выход на новый галс");
  steerToCourse(phase2_target);
  
  // Завершение маневра
  current_tack = new_tack;
  current_tack_course = phase2_target;
  tack_start_time = millis();
  
  Serial.print("✅ Галс изменен на: ");
  Serial.println(current_tack == PORT_TACK ? "Левый борт" : "Правый борт");
}

// Проверка необходимости смены галса
void checkTackConditions() {
  if (current_tack == TACKING) return; // Уже в процессе смены
  
  float wind_direction = getWindDirection();
  float current_heading = compass.getAzimuth();
  unsigned long tack_duration = (millis() - tack_start_time) / 1000;
  
  // 1. Минимальное время на галсе
  if (tack_duration < MIN_TACK_DURATION) return;
  
  // 2. Проверка эффективности галса
  if (!isTackEffective(wind_direction, current_heading)) {
    Serial.println("⚠️ Галс неэффективен - готовим смену");
    prepareForTack();
  }
  
  // 3. Проверка приближения к целевому курсу
  if (isApproachingTargetCourse()) {
    Serial.println("🎯 Приближение к целевому курсу - смена галса");
    performTack();
  }
}

// Основной цикл с системой галсов
void loop() {
  float wind_direction = getWindDirection();
  
  // Обновляем курс для текущего галса
  current_tack_course = calculateTackCourse(wind_direction);
  
  // Проверяем условия смены галса
  checkTackConditions();
  
  // Управляем рулем по курсу галса
  controlRudderToCourse(current_tack_course);
  
  // Отладочная информация
  printTackStatus();
  
  delay(100);
}

// Вывод статуса галса
void printTackStatus() {
  Serial.print("Галс: ");
  switch(current_tack) {
    case PORT_TACK: Serial.print("Левый борт"); break;
    case STARBOARD_TACK: Serial.print("Правый борт"); break;
    case TACKING: Serial.print("СМЕНА ГАЛСА"); break;
    case ON_COURSE: Serial.print("Прямой курс"); break;
  }
  
  Serial.print(" | Курс: ");
  Serial.print(current_tack_course, 1);
  Serial.print("° | Ветер: ");
  Serial.print(getWindDirection(), 1);
  Serial.print("° | Время: ");
  Serial.print((millis() - tack_start_time) / 1000);
  Serial.println("с");
}*/