// ============================================
// ПОЛНАЯ СИСТЕМА УПРАВЛЕНИЯ ЯХТОЙ 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>


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);
}

// ============================================
// ЗЕРКАЛЬНОЕ ПРЕОБРАЗОВАНИЕ С СМЕЩЕНИЕМ 180°
// Для преобразования между разными системами координат
// ============================================

// Функция зеркального отображения с смещением 180°
float convertToMirror(float angle) {
    // Применяем смещение 180° и нормализуем
    float result = angle + 180.0f;
    
    // Нормализация
    if (result >= 360.0f) {
        result -= 360.0f;
    }
    
    // Преобразуем в обратный диапазон [360...0]
    float reversed = 360.0f - result;
    
    // Обработка граничного случая
    return (reversed == 360.0f) ? 0.0f : reversed;
}

// Обратное преобразование
float convertFromMirror(float mirrored) {
    // Из обратного диапазона [360...0] в нормальный
    float normal = 360.0f - mirrored;
    if (normal == 360.0f) normal = 0.0f;
    
    // Отменяем смещение 180°
    float result = normal - 180.0f;
    if (result < 0.0f) result += 360.0f;
    
    return result;
}

// ============================================
// ПРИМЕНЕНИЕ К НАШЕЙ СИСТЕМЕ
// ============================================

// Преобразование морского угла (0°=север) в шкалу X (360→0)
float marineToX(float marine_angle) {
    // Морская система: 0°=север, по часовой стрелке
    // Наша система X: 180°=север, 360°/0°=юг
    
    // 1. Зеркалим относительно вертикали (север-юг)
    float mirrored = convertToMirror(marine_angle);
    
    // 2. Смещаем на 180° (чтобы север стал 180°)
    float X = fmod(mirrored + 180.0f, 360.0f);
    
    return (X < 0.1f) ? 360.0f : X;
}

// Преобразование X в морской угол
float XToMarine(float X) {
    // Обратное преобразование
    float without_offset = fmod(X - 180.0f + 360.0f, 360.0f);
    float marine = convertFromMirror(without_offset);
    
    return marine;
}

// ============================================
// АЛЬТЕРНАТИВНЫЙ РАСЧЕТ Y С ИСПОЛЬЗОВАНИЕМ ЗЕРКАЛИРОВАНИЯ
// ============================================

float calculateY_MirrorMethod(float kurs, float X) {
    // 1. Преобразуем X в морской угол ветра
    float A_wind_marine = XToMarine(X);
    
    // 2. Вычисляем относительный угол в морской системе
    float Y_marine = fmod(A_wind_marine - kurs + 360.0f, 360.0f);
    
    // 3. Преобразуем в нашу систему Y (0/360°=нос)
    float Y = fmod(Y_marine + 180.0f, 360.0f);
    
    return (Y < 0.1f) ? 360.0f : Y;
}

// ============================================
// ЗАГЛУШКИ ДЛЯ ТЕСТИРОВАНИЯ (вместо серво)
// ============================================

struct MotorStub {
    float current_angle = 90.0f; // Текущий угол
    float target_angle = 90.0f;  // Целевой угол
    float speed = 10.0f;         // Скорость изменения, град/сек
    unsigned long last_update = 0;
    
    void setTarget(float angle) {
        target_angle = constrain(angle, 0.0f, 180.0f);
    }
    
    void update() {
        unsigned long now = millis();
        float dt = (now - last_update) / 1000.0f;
        
        if (dt > 0.1f) { // Обновляем не чаще чем раз в 100мс
            float delta = target_angle - current_angle;
            float max_change = speed * dt;
            
            if (fabs(delta) > max_change) {
                current_angle += (delta > 0 ? max_change : -max_change);
            } else {
                current_angle = target_angle;
            }
            
            last_update = now;
        }
    }
    
    float getAngle() {
        return current_angle;
    }
};

MotorStub sailMotor;  // Заглушка для паруса
MotorStub rudderMotor; // Заглушка для руля

// ============================================
// УПРАВЛЕНИЕ С ЗАГЛУШКАМИ (имитация серво)
// ============================================

void initMotorStubs() {
    sailMotor.current_angle = 90.0f;
    sailMotor.target_angle = 90.0f;
    sailMotor.speed = 30.0f; // Быстрее для паруса
    
    rudderMotor.current_angle = 90.0f;
    rudderMotor.target_angle = 90.0f;
    rudderMotor.speed = 15.0f; // Медленнее для руля
}

void controlWithMotorStubs() {
    // УПРАВЛЕНИЕ ПАРУСОМ (имитация серво)
    int sail_target_angle;
    
    if (state.Y > 180.0f) {
        // ЛЕВЫЙ ГАЛС: парус влево
        sail_target_angle = map(state.sail_angle, 0, 90, 90, 0);
    } 
    else if (state.Y > 0.0f && state.Y < 180.0f) {
        // ПРАВЫЙ ГАЛС: парус вправо
        sail_target_angle = map(state.sail_angle, 0, 90, 90, 180);
    }
    else {
        sail_target_angle = 90;
    }
    
    sailMotor.setTarget(sail_target_angle);
    sailMotor.update();
    
    // УПРАВЛЕНИЕ РУЛЕМ (имитация серво)
    int rudder_target_angle = map(state.rudder_angle, 
                                 -boat.max_rudder_angle, 
                                 boat.max_rudder_angle, 
                                 0, 180);
    
    rudderMotor.setTarget(rudder_target_angle);
    rudderMotor.update();
    
    // Вывод текущих положений
    Serial.printf("[Моторы: Парус=%.1f° Руль=%.1f°] ", 
                  sailMotor.getAngle(), rudderMotor.getAngle());
}

// ============================================
// ТЕСТ ЗЕРКАЛЬНОГО ПРЕОБРАЗОВАНИЯ
// ============================================

void testMirrorConversion() {
    Serial.println("\n" + String("=") * 70);
    Serial.println("ТЕСТ ЗЕРКАЛЬНОГО ПРЕОБРАЗОВАНИЯ");
    Serial.println(String("=") * 70);
    Serial.println("Морской° → X° → Морской° → Совпадение");
    Serial.println(String("-") * 70);
    
    float test_angles[] = {0, 45, 90, 135, 180, 225, 270, 315, 360};
    
    for (int i = 0; i < 9; i++) {
        float marine = test_angles[i];
        
        // Прямое преобразование
        float X = marineToX(marine);
        
        // Обратное преобразование
        float marine_back = XToMarine(X);
        
        // Проверка
        bool match = fabs(marine - marine_back) < 0.1f;
        
        Serial.printf("%6.0f° → %4.0f° → %6.0f° → %s\n",
                     marine, X, marine_back, match ? "✓" : "✗");
        delay(50);
    }
    
    // Проверка ключевых точек
    Serial.println("\nКЛЮЧЕВЫЕ ТОЧКИ:");
    Serial.println("Морской | X (наша) | Описание");
    Serial.println("--------|-----------|----------");
    
    struct KeyPoint {
        float marine;
        float expected_X;
        String description;
    };
    
    KeyPoint points[] = {
        {0.0f, 180.0f, "Север → X=180°"},
        {90.0f, 90.0f, "Восток → X=90°"},
        {180.0f, 0.0f, "Юг → X=0°/360°"},
        {270.0f, 270.0f, "Запад → X=270°"},
        {360.0f, 180.0f, "Север(360) → X=180°"}
    };
    
    for (int i = 0; i < 5; i++) {
        float X_calc = marineToX(points[i].marine);
        bool correct = fabs(X_calc - points[i].expected_X) < 0.1f ||
                      (points[i].expected_X == 0.0f && X_calc == 360.0f);
        
        Serial.printf("%6.0f° | %8.0f° | %s %s\n",
                     points[i].marine, X_calc, 
                     points[i].description.c_str(),
                     correct ? "✓" : "✗");
    }
}

// ============================================
// ИНТЕГРАЦИЯ В ОСНОВНУЮ ПРОГРАММУ
// ============================================

// В setup():
void setup() {
    // ... существующий код ...
    initOLED();
    // Инициализация заглушек
    initMotorStubs();
    
    // Тест преобразований
    testMirrorConversion();
    
    // ... остальной код ...
}

// В loop() вместо controlServos():
void loop() {
    static unsigned long last_control = 0;
    
    // ... чтение датчиков ...
    
    // Управление с заглушками каждые 100мс
    if (millis() - last_control >= 100) {
        controlWithMotorStubs();
        last_control = millis();
    }
    
    // ... вывод информации ...
    displayConversionOnOLED()
}

// ============================================
// ВИЗУАЛИЗАЦИЯ ПРЕОБРАЗОВАНИЙ НА OLED
// ============================================

void displayConversionOnOLED() {
    display.clearDisplay();
     
    // Верхняя строка: исходные данные
    display.setCursor(0, 12);
    display.print("Морск:");
    display.print(state.current_course, 0);
    display.print((char)247);
    
    // Средняя строка: преобразование
    display.setCursor(0, 28);
    display.print("X:");
    
    float marine_wind = normalizeAngle(540.0f - state.X_veter);
    float X_calc = marineToX(marine_wind);
    
    display.print(X_calc, 0);
    display.print((char)247);
    
    // Нижняя строка: относительный ветер
    display.setCursor(0, 44);
    display.print("Y:");
    
    float Y_mirror = calculateY_MirrorMethod(state.current_course, state.X_veter);
    display.print(Y_mirror, 0);
    display.print((char)247);
    
    // Статус
    display.setCursor(0, 60);
    float diff = fabs(state.Y - Y_mirror);
    if (diff < 1.0f) {
        display.print("СОВПАДАЕТ ✓");
    } else {
        display.print("РАЗНИЦА:");
        display.print(diff, 1);
        display.print((char)247);
    }
    
    // Диаграмма преобразования
    drawConversionDiagram(marine_wind, X_calc);
    
    display.display();
}

void drawConversionDiagram(float marine, float X) {
    int centerX = 100;
    int centerY = 35;
    int radius = 15;
    
    // Внешний круг (морская система)
    display.drawCircle(centerX, centerY, radius, SSD1306_WHITE);
    
    // Внутренний круг (система X)
    display.drawCircle(centerX, centerY, radius - 4, SSD1306_WHITE);
    
    // Морской угол
    float marine_rad = toRadians(marine - 90); // Поворот для дисплея
    int marineX = centerX + radius * cos(marine_rad);
    int marineY = centerY + radius * sin(marine_rad);
    display.drawLine(centerX, centerY, marineX, marineY, SSD1306_WHITE);
    display.fillCircle(marineX, marineY, 2, SSD1306_WHITE);
    
    // Угол X
    float X_rad = toRadians(X - 90);
    int XX = centerX + (radius - 4) * cos(X_rad);
    int XY = centerY + (radius - 4) * sin(X_rad);
    display.drawLine(centerX, centerY, XX, XY, SSD1306_WHITE);
    
    // Подписи
    display.setFont();
    display.setCursor(centerX - 20, centerY + radius + 5);
    display.print("Морск");
    display.setCursor(centerX + 10, centerY + radius + 5);
    display.print("X");
}

// ============================================
// УНИВЕРСАЛЬНАЯ ФУНКЦИЯ ПРЕОБРАЗОВАНИЯ
// ============================================

// Универсальное преобразование между системами
struct CoordinateSystem {
    float zero_angle;      // Где находится 0° в этой системе
    bool clockwise;        // Направление увеличения (true = по ЧС)
    bool reverse_scale;    // Обратная шкала (360→0)
};

CoordinateSystem marine_system = {0.0f, true, false};    // 0°=север, по ЧС
CoordinateSystem X_system = {180.0f, false, true};       // 180°=север, обратная шкала
CoordinateSystem Y_system = {0.0f, true, false};         // 0°/360°=нос

float convertBetweenSystems(float angle, CoordinateSystem from, CoordinateSystem to) {
    // 1. Приводим к стандартной системе (0°=север, по ЧС)
    float standard = angle;
    
    // Учитываем начало отсчёта
    standard = fmod(standard - from.zero_angle + 360.0f, 360.0f);
    
    // Учитываем направление
    if (!from.clockwise) {
        standard = 360.0f - standard;
    }
    
    // Учитываем обратную шкалу
    if (from.reverse_scale) {
        standard = 360.0f - standard;
    }
    
    // 2. Преобразуем в целевую систему
    float result = standard;
    
    // Учитываем обратную шкалу целевой системы
    if (to.reverse_scale) {
        result = 360.0f - result;
    }
    
    // Учитываем направление целевой системы
    if (!to.clockwise) {
        result = 360.0f - result;
    }
    
    // Учитываем начало отсчёта целевой системы
    result = fmod(result + to.zero_angle, 360.0f);
    
    return (result < 0.1f) ? 360.0f : result;
}

void testUniversalConversion() {
    Serial.println("\nУНИВЕРСАЛЬНОЕ ПРЕОБРАЗОВАНИЕ:");
    
    float test_angle = 90.0f; // Восток в морской системе
    
    float to_X = convertBetweenSystems(test_angle, marine_system, X_system);
    float back = convertBetweenSystems(to_X, X_system, marine_system);
    
    Serial.printf("Морской %.0f° → X %.0f° → Морской %.0f° %s\n",
                 test_angle, to_X, back,
                 fabs(test_angle - back) < 0.1f ? "✓" : "✗");
}