// ============================================
// ПОЛНАЯ СИСТЕМА УПРАВЛЕНИЯ ЯХТОЙ ESP32
// Управление парусом + автопилот руля
// Исправленная версия с корректной логикой режимов
// ============================================
// Конфигурация OLED дисплея
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C

#include <Wire.h>
#include <math.h>
#include <ESP32Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//#include <Fonts/FreeSerif9pt7b.h>

// Конфигурация пинов ESP32
const int PIN_POT_KURS = 34;        // Потенциометр заданного курса
const int PIN_POT_VETER = 35;       // Потенциометр направления ветра
const int PIN_POT_CURRENT_KURS = 32; // Текущий курс (с компаса)
const int PIN_SERVO_SAIL = 13;      // Сервопривод паруса
const int PIN_SERVO_RUDDER = 12;    // Сервопривод руля
const int PIN_BUTTON_MODE = 14;     // Кнопка смены режима

// Режимы работы
enum ControlMode {
    MODE_MANUAL,      // Ручное управление
    MODE_AUTO_SAIL,   // Авто парус + ручной руль
    MODE_AUTO_RUDDER, // Авто руль + ручной парус
    MODE_FULL_AUTO    // Полный автопилот
};

ControlMode currentMode = MODE_FULL_AUTO;
bool left_tack = false;  // Глобальный флаг левого галса

// Параметры судна
struct Boat {
    float length = 8.0;          // Длина, м
    float max_rudder_angle = 30.0; // Макс. угол руля, градусы
    float rudder_sensitivity = 2.0; // Чувствительность руля
    float turning_radius = 20.0;   // Радиус циркуляции, м
    float response_time = 3.0;     // Время реакции, сек
};

Boat boat;

// Состояние системы
struct SystemState {
    // Датчики
    float target_course = 0.0;     // Заданный курс
    float current_course = 0.0;    // Текущий курс (с компаса)
    float wind_direction = 0.0;    // Направление ветра (шкала X)
    float wind_speed = 5.0;        // Скорость ветра, м/с
    
    // Вычисленные
    float relative_wind = 0.0;     // Относительный ветер Y
    float sail_angle = 0.0;        // Угол паруса
    float sail_side = 0.0;         // Сторона паруса (+ вправо, - влево)
    float rudder_angle = 0.0;      // Угол руля
    
    // Ошибки и производные
    float course_error = 0.0;      // Ошибка курса
    float course_error_integral = 0.0; // Интеграл ошибки
    float course_error_derivative = 0.0; // Производная ошибки
    
    // PID коэффициенты
    float Kp = 2.0;    // Пропорциональный
    float Ki = 0.1;    // Интегральный
    float Kd = 1.0;    // Дифференциальный
    
    // Ограничения
    float max_integral = 50.0;     // Антивинд-ап интеграла
};

const int pinR = 33;
const int pinG = 25;
const int pinB = 26;

SystemState state;
Servo sailServo;
Servo rudderServo;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// ============================================
// ИНИЦИАЛИЗАЦИЯ OLED
// ============================================
void initOLED() {
    if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
        Serial.println("OLED не инициализирован!");
        while (1);
    }
    
    display.clearDisplay();
    display.setTextColor(SSD1306_WHITE);
    //display.setFont(&FreeSerif9pt7b);
    display.setTextSize(1);
    
    // Заставка
    display.clearDisplay();
    display.setCursor(0, 20);
    display.print("Yaht");
    display.setCursor(0, 40);
    display.print("Autopilot");
    display.display();
    delay(2000);
}

// ============================================
// КОМПАКТНЫЙ ВЫВОД НА OLED
// ============================================

void updateOLED() {
    display.clearDisplay();
    
    // Строка 1: Режим и курс
    display.setCursor(0, 0);
    display.print(getModeName(currentMode).substring(0, 8));
    
    display.setCursor(70, 0);
    display.print(state.current_course, 0);
    display.print((char)247); // Символ градуса
    
    // Строка 2: Ветер
    display.setCursor(0, 16);
    float true_wind = normalizeAngle(540.0 - state.wind_direction);
    display.print("V:");
    display.print(true_wind, 0);
    display.print((char)247);
    
    // Строка 3: Относительный ветер и режим
    display.setCursor(0, 32);
    display.print("Y:");
    display.print(state.relative_wind, 0);
    display.print((char)247);
    
    String mode_short = getSailMode(state.relative_wind).substring(0, 4);
    display.setCursor(70, 32);
    display.print(mode_short);
    
    // Строка 4: Парус и руль
    display.setCursor(0, 48);
    display.print("P:");
    display.print(state.sail_angle, 0);
    display.print((char)247);
    
    display.setCursor(70, 48);
    display.print("R:");
    display.print(state.rudder_angle, 0);
    display.print((char)247);
    
    // Индикатор галса (стрелка)
    int indicator_x = 110;
    if (state.relative_wind > 180) {
        // Левый галс ←
        display.setCursor(0, 80);
        display.print("←");
    } else if (state.relative_wind > 0 && state.relative_wind < 180) {
        // Правый галс →
        display.setCursor(0, 80);
        display.print("→");
    } else if (state.relative_wind == 180) {
        // В корму ↓
        display.setCursor(0, 0);
        display.print("↓");
    } else {
        // В нос ↑
        display.setCursor(0, 80);
        display.print("↑");
    }
    
    display.display();
}

// ============================================
// МАТЕМАТИЧЕСКИЕ ФУНКЦИИ
// ============================================

// Преобразование градусов в радианы
float toRadians(float degrees) {
    return degrees * M_PI / 180.0;
}

// Преобразование радиан в градусы
float toDegrees(float radians) {
    return radians * 180.0 / M_PI;
}

// Нормализация угла в диапазон 0-360
float normalizeAngle(float angle) {
    angle = fmod(angle, 360.0);
    if (angle < 0) angle += 360.0;
    return angle;
}

// ============================================
// ФУНКЦИИ ЧТЕНИЯ ДАТЧИКОВ
// ============================================

void readSensors() {
    // Чтение потенциометров
    state.target_course = map(analogRead(PIN_POT_KURS), 0, 4095, 0, 359);
    state.wind_direction = map(analogRead(PIN_POT_VETER), 0, 4095, 360, 0);
    state.current_course = map(analogRead(PIN_POT_CURRENT_KURS), 0, 4095, 0, 359);
    
    // Ограничения
    state.target_course = constrain(state.target_course, 0, 359);
    state.wind_direction = constrain(state.wind_direction, 0, 360);
    state.current_course = constrain(state.current_course, 0, 359);
    
    // Чтение кнопки режима
    static unsigned long last_press = 0;
    if (digitalRead(PIN_BUTTON_MODE) == LOW && 
        millis() - last_press > 500) {
        currentMode = (ControlMode)((currentMode + 1) % 4);
        last_press = millis();
        Serial.print("\nРежим изменён на: ");
        Serial.println(getModeName(currentMode));
    }
}

// ============================================
// УПРАВЛЕНИЕ ПАРУСОМ (исправленная логика)
// ============================================

// Вычисление относительного ветра Y
float calculateWindY(float kurs, float X_veter) {
    float A_veter = normalizeAngle(540.0 - X_veter);
    float theta = normalizeAngle(A_veter - kurs);
    return (theta < 0.1) ? 360.0 : theta;
}

// Определение режима хода согласно таблице
String getSailMode(float Y) {
    // Приводим к диапазону 0-360
    float normY = Y;
    if (Y == 360) normY = 0;
    
    // Определяем галс
    left_tack = (normY > 180);
    
    // Если левый галс, зеркалим для определения режима
    if (left_tack) {
        normY = 360 - normY;
    }
    
    // Классификация по таблице
    if (normY <= 30) {
        return "BEIDEWIND"; //"БЕЙДЕВИНД";
    } 
    else if (normY <= 90) {
        return "GULFWIND"; //"ГАЛФВИНД";
    } 
    else if (normY <= 150) {
        return "BACKSTAY"; //"БАКШТАГ";
    } 
    else if (normY <= 210) {
        return "FORDWIND"; //"ФОРДЕВИНД";
    }
    else if (normY <= 270) {
        // Для диапазона 210-270 (зеркальный галс)
        return "GULFWIND"; //"ГАЛФВИНД"; // Ветер сбоку с левого борта
    }
    else {
        // Для диапазона 270-330 (зеркальный галс)
        return "BEIDEWIND"; //"БЕЙДЕВИНД"; // Ветер почти в нос с левого борта
    }
}

// Вычисление угла паруса согласно таблице
float calculateSailAngle(float Y) {
    float normY = Y;
    if (Y == 360) normY = 0;
    
    // Определяем галс
    left_tack = (normY > 180);
    
    // Зеркалим для расчета (приводим к правому борту)
    if (left_tack) {
        normY = 360 - normY;
    }
    
    float angle;
    
    // Расчет согласно таблице оптимальных углов
    if (normY <= 30) {
        // Бейдевинд: 15-30°
        angle = 15.0 + normY * 0.5; // 15° при 0°, 30° при 30°
    } 
    else if (normY <= 90) {
        // Галфвинд: 30-60°
        angle = 30.0 + (normY - 30) * 0.5; // 30° при 30°, 60° при 90°
    } 
    else if (normY <= 150) {
        // Бакштаг: 60-90°
        angle = 60.0 + (normY - 90) * 0.5; // 60° при 90°, 90° при 150°
    } 
    else if (normY <= 210) {
        // Фордевинд: 90°
        angle = 90.0;
    }
    else if (normY <= 270) {
        // Галфвинд с др.борта: 30-60°
        // Для норм.Y 210-270 соответствует Y 150-90° на левом борту
        float mirroredY = 360 - normY; // Обратное зеркалирование
        if (mirroredY <= 90) {
            angle = 30.0 + mirroredY * 0.33; // 30-60°
        } else {
            angle = 60.0;
        }
    }
    else {
        // Бейдевинд с др.борта: 15-30°
        // Для норм.Y 270-330 соответствует Y 90-30° на левом борту
        float mirroredY = 360 - normY;
        if (mirroredY <= 30) {
            angle = 15.0 + mirroredY * 0.5; // 15-30°
        } else {
            angle = 30.0;
        }
    }
    
    // Ограничение
    angle = constrain(angle, 0.0, 90.0);
    
    // Определяем сторону паруса (положительная = вправо)
    state.sail_side = left_tack ? -1.0 : 1.0;
    
    return angle;
}

// ============================================
// АВТОПИЛОТ РУЛЯ (PID КОНТРОЛЛЕР)
// ============================================

// Вычисление ошибки курса с учётом круговой природы
float calculateCourseError(float target, float current) {
    float error = target - current;
    
    // Коррекция для круговой системы (кратчайший путь)
    if (error > 180.0) error -= 360.0;
    if (error < -180.0) error += 360.0;
    
    return error;
}

// ПИД-регулятор для управления рулём
float calculateRudderAngle(float error) {
    static unsigned long last_time = 0;
    unsigned long current_time = millis();
    float dt = (current_time - last_time) / 1000.0;
    
    if (last_time == 0 || dt > 1.0) {
        dt = 0.1; // Защита от больших dt
    }
    
    // Интегральная составляющая
    state.course_error_integral += error * dt;
    
    // Антивинд-ап интеграла
    if (state.course_error_integral > state.max_integral) 
        state.course_error_integral = state.max_integral;
    if (state.course_error_integral < -state.max_integral) 
        state.course_error_integral = -state.max_integral;
    
    // Дифференциальная составляющая
    float derivative = 0;
    if (dt > 0) {
        derivative = (error - state.course_error) / dt;
        state.course_error_derivative = derivative;
    }
    
    // Сохраняем ошибку для следующего шага
    state.course_error = error;
    
    // ПИД формула
    float output = state.Kp * error + 
                   state.Ki * state.course_error_integral + 
                   state.Kd * derivative;
    
    last_time = current_time;
    
    return output;
}

// Компенсация влияния ветра на руль
float compensateWindEffect(float rudder_angle, float relative_wind) {
    // Ветер создаёт дополнительный момент
    float wind_compensation = 0;
    
    // Учитываем и относительный ветер, и сторону паруса
    float effective_wind = relative_wind;
    if (left_tack) {
        effective_wind = 360 - relative_wind;
    }
    
    if (effective_wind > 0 && effective_wind < 180) {
        // Ветер справа - нужно больше левого руля
        wind_compensation = -sin(toRadians(effective_wind)) * 3.0;
    } else if (effective_wind > 180) {
        // Ветер слева - нужно больше правого руля
        wind_compensation = sin(toRadians(effective_wind - 180)) * 3.0;
    }
    
    // Усиливаем компенсацию при большом угле паруса
    if (state.sail_angle > 60.0) {
        wind_compensation *= 1.5;
    }
    
    return rudder_angle + wind_compensation;
}

// Автоматическое управление рулём
void autoRudderControl() {
    // Вычисляем ошибку курса
    float error = calculateCourseError(state.target_course, state.current_course);
    
    // Вычисляем угол руля по ПИД
    float rudder_angle = calculateRudderAngle(error);
    
    // Компенсация влияния ветра
    rudder_angle = compensateWindEffect(rudder_angle, state.relative_wind);
    
    // Ограничение угла руля
    state.rudder_angle = constrain(rudder_angle, 
                                  -boat.max_rudder_angle, 
                                  boat.max_rudder_angle);
}

// Ручное управление рулём (через потенциометр)
void manualRudderControl() {
    state.rudder_angle = 0; // В ручном режиме руль по центру
}

// ============================================
// УПРАВЛЕНИЕ СЕРВОПРИВОДАМИ
// ============================================

void initServos() {
    // Инициализация сервоприводов
    sailServo.attach(PIN_SERVO_SAIL);
    rudderServo.attach(PIN_SERVO_RUDDER);
    
    // Установка начального положения
    sailServo.write(90);  // Парус по центру
    rudderServo.write(90); // Руль по центру
    delay(500);
}

void controlServos() {
    // Управление сервоприводом паруса
    // Для правого галса: 90° = вдоль ДП, >90° = вправо, <90° = влево
    int sail_servo_angle;
    if (left_tack) {
        // Левый галс: парус влево
        sail_servo_angle = map(state.sail_angle, 0, 90, 90, 0);
    } else {
        // Правый галс: парус вправо
        sail_servo_angle = map(state.sail_angle, 0, 90, 90, 180);
    }
    sailServo.write(sail_servo_angle);
    
    // Управление сервоприводом руля
    // 90° = прямо, >90° = вправо, <90° = влево
    int rudder_servo_angle = map(state.rudder_angle, 
                                -boat.max_rudder_angle, 
                                boat.max_rudder_angle, 
                                0, 180);
    rudderServo.write(rudder_servo_angle);
    
    // Задержка для сервоприводов
    delay(15);
}

// ============================================
// ОСНОВНАЯ ЛОГИКА УПРАВЛЕНИЯ
// ============================================

void controlSystem() {
    // Вычисляем относительный ветер
    state.relative_wind = calculateWindY(state.current_course, state.wind_direction);
    
    // Управление парусом (всегда автоматическое если не ручной режим)
    if (currentMode != MODE_MANUAL) {
        state.sail_angle = calculateSailAngle(state.relative_wind);
    }
    
    // Управление рулём в зависимости от режима
    switch (currentMode) {
        case MODE_MANUAL:
            manualRudderControl();
            break;
            
        case MODE_AUTO_SAIL:
            manualRudderControl(); // Руль вручную
            break;
            
        case MODE_AUTO_RUDDER:
            autoRudderControl(); // Автопилот руля
            break;
            
        case MODE_FULL_AUTO:
            autoRudderControl(); // Полный автопилот
            break;
    }
    
    // Управление сервоприводами
    controlServos();
}

// ============================================
// ВЫВОД ИНФОРМАЦИИ
// ============================================

String getModeName(ControlMode mode) {
    switch (mode) {
        case MODE_MANUAL:  {
           digitalWrite(pinR, 1);
           digitalWrite(pinG, 0);
           digitalWrite(pinB, 0);
           return "MANUAL";//"РУЧНОЙ";
        }   
        case MODE_AUTO_SAIL: {
          digitalWrite(pinR, 1);
          digitalWrite(pinG, 1);
          digitalWrite(pinB, 0);
          return "ASAIL"; //"АВТО ПАРУС";
        }  
        case MODE_AUTO_RUDDER: {
          digitalWrite(pinR, 0);
          digitalWrite(pinG, 0);
          digitalWrite(pinB, 1);
          return "ARUDDER"; //"АВТО РУЛЬ";
        }
        case MODE_FULL_AUTO: {
          digitalWrite(pinR, 0);
          digitalWrite(pinG, 1);
          digitalWrite(pinB, 0);
          return "AUTO"; //"ПОЛНЫЙ АВТО";
        }  
        default:   {
           digitalWrite(pinR, 1);
           digitalWrite(pinB, 1);
           return "НЕИЗВЕСТНО";
        }           
    }
}

String getWindSide(float Y) {
    if (Y == 0 || Y == 360) return "NOSE"; //"В НОС";
    else if (Y == 180) return "FEED"; //"В КОРМУ";
    else if (Y > 0 && Y < 180) return "RIGHT BOARD"; //"ПРАВ.БОРТ";
    else return "LEFT BOARD"; //"ЛЕВ.БОРТ";
}

String getSailDirection() {
    if (state.sail_angle < 5.0) return "MIDLE"; //"ПО ДП";
    else if (left_tack) return "TO LEFT"; //"ВЛЕВО";
    else return "TO RIGHT"; //"ВПРАВО";
}

String getRudderDirection() {
    if (fabs(state.rudder_angle) < 1.0) return "STRAIGHT"; //"ПРЯМО";
    else if (state.rudder_angle > 0) return "TO RIGHT"; //"ВПРАВО";
    else return "TO LEFT"; //"ВЛЕВО";
}

void printDashboard() {
    Serial.println("\n========================================");
    Serial.println("           АВТОПИЛОТ ЯХТЫ");
    Serial.println("========================================");
    
    // Режим работы
    Serial.printf("РЕЖИМ: %s\n", getModeName(currentMode).c_str());
    Serial.println("----------------------------------------");
    
    // Курсы
    Serial.printf("Заданный курс: %3.0f° | Текущий курс: %3.0f°\n", 
                  state.target_course, state.current_course);
    Serial.printf("Ошибка курса: %+5.1f°\n", state.course_error);
    
    // Ветер
    float true_wind = normalizeAngle(540.0 - state.wind_direction);
    String sail_mode = getSailMode(state.relative_wind);
    Serial.printf("Ветер: %3.0f° (X=%3.0f) | Относительный: %3.0f° (%s)\n",
              true_wind, state.wind_direction,
              state.relative_wind, getWindSide(state.relative_wind).c_str());
    Serial.printf("Режим хода: %s | Галс: %s\n", 
                  sail_mode.c_str(), 
                  left_tack ? "ЛЕВЫЙ" : "ПРАВЫЙ");

    // Управляющие поверхности
    Serial.println("----------------------------------------");
    Serial.printf("ПАРУС: %5.1f° (%s) | РУЛЬ: %+5.1f° (%s)\n", 
                  state.sail_angle, getSailDirection().c_str(),
                  state.rudder_angle, getRudderDirection().c_str());
    
    // Визуальные индикаторы
    Serial.println("----------------------------------------");
    
    // Индикатор курса
    Serial.print("Заданный курс: ");
    for (int i = 0; i < 36; i++) {
        float angle = i * 10;
        if (fabs(angle - state.target_course) < 5) {
            Serial.print("◉");
        } else {
            Serial.print("·");
        }
    }
    Serial.println();
    
    // Индикатор руля
    Serial.print("Руль: [");
    int rudder_pos = map((int)state.rudder_angle, 
                        -boat.max_rudder_angle, 
                        boat.max_rudder_angle, 
                        0, 40);
    for (int i = 0; i < 40; i++) {
        if (i == 20) Serial.print("|");
        else if (i == rudder_pos) Serial.print("▶");
        else Serial.print(" ");
    }
    Serial.println("]");
    
    // Индикатор паруса
    Serial.print("Парус: [");
    int sail_pos = map((int)state.sail_angle, 0, 90, 0, 40);
    for (int i = 0; i < 40; i++) {
        if (i == 20) Serial.print("|");
        else if (i == sail_pos) {
            if (left_tack) Serial.print("◀");
            else Serial.print("▶");
        }
        else Serial.print(" ");
    }
    Serial.printf("] %s\n", getSailDirection().c_str());
    
    // ПИД составляющие
    Serial.println("----------------------------------------");
    Serial.printf("ПИД: P=%+5.1f I=%+5.1f D=%+5.1f\n",
                  state.Kp * state.course_error,
                  state.Ki * state.course_error_integral,
                  state.Kd * state.course_error_derivative);
    
    // Рекомендации
    Serial.println("----------------------------------------");
    if (fabs(state.course_error) > 10.0) {
        Serial.println("⚠️  Большая ошибка курса! Проверьте настройки ПИД.");
    } else if (fabs(state.course_error) < 2.0) {
        Serial.println("✅ Курс удерживается отлично!");
    } else {
        Serial.println("📊 Курс в допустимых пределах.");
    }
    
    // Управление
    Serial.println("----------------------------------------");
    Serial.println("Кнопка GPIO14 - смена режима");
    Serial.println("========================================\n");
}

// ============================================
// ТЕСТОВЫЕ ФУНКЦИИ
// ============================================

// Тест всех режимов паруса
void testAllSailModes() {
    Serial.println("\n=== ТЕСТ РЕЖИМОВ ПАРУСА ===");
    Serial.println("Y° | Режим       | Парус° | Галс     | Направление");
    Serial.println("--------------------------------------------------");
    
    for (int Y = 0; Y <= 360; Y += 30) {
        int displayY = (Y == 360) ? 360 : Y;
        float rel_wind = displayY;
        String mode = getSailMode(rel_wind);
        float angle = calculateSailAngle(rel_wind);
        String tack = (rel_wind == 0 || rel_wind == 360) ? "NOSE" : 
                     (rel_wind == 180) ? "КОРМА" :
                     (rel_wind > 0 && rel_wind < 180) ? "RIGHT" : "LEFT";
        String direction = getSailDirection();
        
        Serial.printf("%3d° | %-12s | %6.1f° | %-8s | %s\n", 
                     displayY, mode.c_str(), angle, tack.c_str(), direction.c_str());
        delay(100);
    }
    
    Serial.println("\n=== ТАБЛИЦА СООТВЕТСТВИЯ ===");
    Serial.println("Y°       | Режим              | Угол паруса");
    Serial.println("---------|--------------------|------------");
    Serial.println("0-30     | БЕЙДЕВИНД          | 15-30°");
    Serial.println("30-90    | ГАЛФВИНД           | 30-60°");
    Serial.println("90-150   | БАКШТАГ            | 60-90°");
    Serial.println("150-210  | ФОРДЕВИНД          | 90°");
    Serial.println("210-270  | ГАЛФВИНД с др.борта| 30-60°");
    Serial.println("270-330  | БЕЙДЕВИНД с др.борта| 15-30°");
}

// Калибровка ПИД-регулятора
void calibratePID() {
    Serial.println("\n=== КАЛИБРОВКА ПИД РЕГУЛЯТОРА ===");
    
    float test_Kp[] = {1.0, 2.0, 3.0};
    float test_Ki[] = {0.05, 0.1, 0.2};
    float test_Kd[] = {0.5, 1.0, 2.0};
    
    for (int i = 0; i < 3; i++) {
        state.Kp = test_Kp[i];
        state.Ki = test_Ki[i];
        state.Kd = test_Kd[i];
        
        Serial.printf("Тест: Kp=%.1f, Ki=%.2f, Kd=%.1f\n", 
                     state.Kp, state.Ki, state.Kd);
        
        // Тестовый сценарий
        state.target_course = 45.0;
        state.current_course = 0.0;
        
        for (int step = 0; step < 10; step++) {
            autoRudderControl();
            Serial.printf("  Шаг %d: ошибка=%.1f°, руль=%.1f°\n", 
                         step, state.course_error, state.rudder_angle);
            delay(200);
        }
    }
}

// Тест сервоприводов
void testServos() {
    Serial.println("\n=== ТЕСТ СЕРВОПРИВОДОВ ===");
    
    // Тест паруса
    Serial.println("Парус: центрирование");
    sailServo.write(90);
    delay(1000);
    
    Serial.println("Парус: вправо 45°");
    sailServo.write(135);
    delay(1000);
    
    Serial.println("Парус: влево 45°");
    sailServo.write(45);
    delay(1000);
    
    Serial.println("Парус: центрирование");
    sailServo.write(90);
    delay(1000);
    
    // Тест руля
    Serial.println("Руль: центрирование");
    rudderServo.write(90);
    delay(1000);
    
    Serial.println("Руль: вправо 30°");
    rudderServo.write(120);
    delay(1000);
    
    Serial.println("Руль: влево 30°");
    rudderServo.write(60);
    delay(1000);
    
    Serial.println("Руль: центрирование");
    rudderServo.write(90);
    delay(1000);
}

// ============================================
// НАСТРОЙКА И ОСНОВНОЙ ЦИКЛ
// ============================================

void setup() {
    Serial.begin(115200);
    delay(1000);

    pinMode(pinR, OUTPUT);
    pinMode(pinG, OUTPUT);
    pinMode(pinB, OUTPUT);
    
    initOLED();
    
    Serial.println("\n\n🚢 АВТОПИЛОТ ПАРУСНОЙ ЯХТЫ 🚢");
    Serial.println("ESP32 | Управление парусом и рулём");
    Serial.println("Версия 3.0 - Исправленная логика режимов");
    Serial.println("========================================\n");
    
    // Настройка пинов
    pinMode(PIN_POT_KURS, INPUT);
    pinMode(PIN_POT_VETER, INPUT);
    pinMode(PIN_POT_CURRENT_KURS, INPUT);
    pinMode(PIN_BUTTON_MODE, INPUT_PULLUP);
    
    // Настройка АЦП
    analogReadResolution(12);
    analogSetAttenuation(ADC_11db);
    
    // Инициализация сервоприводов
    initServos();
    
    // Тестовые функции (закомментируйте после отладки)
    // testAllSailModes();
    // testServos();
    
    Serial.println("\nСистема готова. Режимы:");
    Serial.println("1. РУЧНОЙ - полное ручное управление");
    Serial.println("2. АВТО ПАРУС - парус автомат, руль вручную");
    Serial.println("3. АВТО РУЛЬ - руль автомат, парус вручную");
    Serial.println("4. ПОЛНЫЙ АВТО - полностью автоматически");
    Serial.println("\nКнопка на GPIO14 - смена режима");
    Serial.println("========================================\n");
    delay(2000);
}

void loop() {
    static unsigned long last_update = 0;
    const int UPDATE_INTERVAL = 1000; // Обновление раз в секунду
    
    // Чтение датчиков
    readSensors();
    
    // Управление системой
    controlSystem();
    
    // Вывод информации
    if (millis() - last_update >= UPDATE_INTERVAL) {
        printDashboard();
        last_update = millis();
    }

    updateOLED();
    
    // Небольшая задержка
    delay(50);
}