#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("с");
}