// ============================================
// ПРОГРАММА УПРАВЛЕНИЯ ПАРУСОМ ДЛЯ ESP32
// Версия для 3.3В и 12-битного АЦП
// ============================================

// Конфигурация пинов ESP32
const int PIN_POT_KURS = 34;    // GPIO34 (ADC1_CH6) - потенциометр курса
const int PIN_POT_VETER = 35;   // GPIO35 (ADC1_CH7) - потенциометр ветра
const int PIN_SERVO = 13;       // GPIO13 - сервопривод (ШИМ)

// Для сервопривода (если используете)
#include <ESP32Servo.h>
Servo sailServo;

// Калибровочные константы для 3.3В
const float ADC_MAX = 4095.0;    // 12-битный АЦП ESP32
const float VOLTAGE_MAX = 3.3;   // Максимальное напряжение

// Переменные
int kurs = 0;           // Курс судна (0-360°)
int X_veter = 0;        // Показание флюгера (шкала X: 360→0)
int Y = 0;              // Относительный ветер (0-360)
float sail_angle = 0;   // Угол паруса (0-90° от ДП)
bool left_tack = false; // Флаг левого галса

// ============================================
// ФУНКЦИИ ЧТЕНИЯ АНАЛОГОВЫХ ДАТЧИКОВ (3.3В)
// ============================================

// Чтение аналогового значения с фильтром
int readAnalogFiltered(int pin, int samples = 10) {
    long sum = 0;
    for (int i = 0; i < samples; i++) {
        sum += analogRead(pin);
        delay(1);
    }
    return sum / samples;
}

// Чтение и калибровка потенциометров
void readSensors() {
    // Читаем сырые значения АЦП (0-4095)
    int raw_kurs = readAnalogFiltered(PIN_POT_KURS);
    int raw_veter = readAnalogFiltered(PIN_POT_VETER);
    
    // Преобразуем в напряжение (для отладки)
    float volt_kurs = (raw_kurs / ADC_MAX) * VOLTAGE_MAX;
    float volt_veter = (raw_veter / ADC_MAX) * VOLTAGE_MAX;
    
    // Преобразуем в градусы
    kurs = map(raw_kurs, 0, 4095, 0, 359);
    X_veter = map(raw_veter, 0, 4095, 360, 0); // Инверсная шкала!
    
    // Ограничения
    kurs = constrain(kurs, 0, 359);
    X_veter = constrain(X_veter, 0, 360);
    
    // Отладка напряжения (опционально)
    /*
    Serial.print("Напряжения: Курс=");
    Serial.print(volt_kurs, 2);
    Serial.print("В, Ветер=");
    Serial.print(volt_veter, 2);
    Serial.println("В");
    */
}

// ============================================
// ВЫЧИСЛИТЕЛЬНЫЕ ФУНКЦИИ (без изменений)
// ============================================

// Вычисление относительного ветра Y
int calculateWindY(int kurs, int X_veter) {
    int theta = (540 - X_veter - kurs) % 360;
    if (theta < 0) theta += 360;
    return (theta == 0) ? 360 : theta;
}

// Определение галса
String getTack(int Y) {
    if (Y == 360 || Y == 0) return "В НОС";
    else if (Y > 0 && Y < 180) return "ПРАВ.ГАЛС";
    else if (Y > 180 && Y < 360) return "ЛЕВ.ГАЛС";
    else if (Y == 180) return "В КОРМУ";
    else return "ОШИБКА";
}

// Определение режима хода
String getSailMode(int Y) {
    int normY = Y;
    if (Y == 360) normY = 0;
    
    // Зеркалим для левого борта
    if (normY > 180) normY = 360 - normY;
    
    if (normY <= 45) {                    // Было <=30
        return "БЕЙДЕВИНД";
    } 
    else if (normY <= 120) {              // Было <=90
        return "ГАЛФВИНД";
    } 
    else if (normY <= 180) {              // Было <=150
        return "БАКШТАГ";
    } 
    else {
        return "ОШИБКА";
    }
}

float calculateSailAngle(int Y) {
    int normY = Y;
    if (Y == 360) normY = 0;
    
    left_tack = (normY > 180);
    if (left_tack) {
        normY = 360 - normY;
    }
    
    float angle;
    if (normY <= 45) {
        // Бейдевинд до 45° относительного ветра
        angle = 15.0 + normY * 0.33; // От 15° до 30°
    } 
    else if (normY <= 120) {
        // Галфвинд 45-120°
        angle = 30.0 + (normY - 45) * 0.4; // От 30° до 60°
    } 
    else {
        // Бакштаг и фордевинд
        angle = 60.0 + (normY - 120) * 0.5; // От 60° до 90°
        if (angle > 90.0) angle = 90.0;
    }
    
    return constrain(angle, 0.0, 90.0);
}

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

void initServo() {
    
    // Раскомментировать если используете сервопривод
    ESP32PWM::allocateTimer(0);
    sailServo.setPeriodHertz(50);      // Стандартная частота серво
    sailServo.attach(PIN_SERVO, 500, 2400); // Диапазон импульсов
   
}

void controlServo(float angle) {
    
    // Преобразуем угол паруса в угол сервопривода
    int servo_angle = constrain((int)angle, 0, 90);
    
    // Учитываем галс (для двухбортовой системы)
    if (left_tack) {
        // Для левого галса инвертируем или используем другой сервопривод
        servo_angle = 180 - servo_angle;
    }
    
    sailServo.write(servo_angle);
    delay(15); // Даём время серво на перемещение
   
}

// ============================================
// ВЫВОД ИНФОРМАЦИИ
// ============================================
void printInfo() {
    // Компактный вывод
    Serial.printf("Курс:%3d° Флюгер:%3d Y:%3d° %-9s %-12s Парус:%5.1f°", 
                  kurs, X_veter, Y, 
                  getTack(Y).c_str(), 
                  getSailMode(Y).c_str(), 
                  sail_angle);
    
    // Индикация галса
    if (left_tack) {
        Serial.print(" [ЛЕВЫЙ]");
    } else if (Y > 0 && Y < 180) {
        Serial.print(" [ПРАВЫЙ]");
    } else if (Y == 0 || Y == 360) {
        Serial.print(" [В НОС]");
    } else if (Y == 180) {
        Serial.print(" [В КОРМУ]");
    }
    
    Serial.println();
}

void testSailAngles() {
    Serial.println("\n=== ТЕСТ РАСЧЁТА УГЛОВ ПАРУСА ===");
    Serial.println("Y° | Режим       | Парус° | Галс");
    Serial.println("---------------------------------");
    
    int test_points[] = {0, 30, 45, 90, 120, 150, 180, 210, 270, 315, 360};
    
    for (int i = 0; i < 11; i++) {
        int Y = test_points[i];
        String mode = getSailMode(Y);
        float angle = calculateSailAngle(Y);
        String tack = getTack(Y);
        
        Serial.printf("%3d° | %-12s | %6.1f° | %s\n", 
                     Y, mode.c_str(), angle, tack.c_str());
    }
    delay(5000);
}

// ============================================
// НАСТРОЙКА ESP32
// ============================================

void setup() {
    // Инициализация Serial
    Serial.begin(115200); // ESP32 использует 115200 по умолчанию
    delay(1000);
    
    Serial.println("\n\n==================================");
    Serial.println("   СИСТЕМА УПРАВЛЕНИЯ ПАРУСОМ");
    Serial.println("         ESP32 v1.0");
    Serial.println("    Рабочее напряжение: 3.3В");
    Serial.println("==================================\n");
    
    // Настройка пинов
    pinMode(PIN_POT_KURS, INPUT);
    pinMode(PIN_POT_VETER, INPUT);
    
    // Настройка АЦП (опционально)
    analogReadResolution(12);     // 12 бит (0-4095)
    analogSetAttenuation(ADC_11db); // Полный диапазон 0-3.3В
    
    // Инициализация сервопривода
    initServo();

    testSailAngles();
    
    // Тестовое сообщение
    Serial.println("Система готова. Крутите потенциометры:");
    Serial.println("  GPIO34 (A0) - Курс судна");
    Serial.println("  GPIO35 (A1) - Направление ветра");
    Serial.println("----------------------------------------");
    delay(2000);
  
}

// ============================================
// ОСНОВНОЙ ЦИКЛ
// ============================================

void loop() {
    static unsigned long lastPrint = 0;
    const int PRINT_INTERVAL = 300; // мс
    
    // 1. Чтение датчиков
    readSensors();
    
    // 2. Вычисление относительного ветра
    Y = calculateWindY(kurs, X_veter);
    
    // 3. Вычисление угла паруса
    sail_angle = calculateSailAngle(Y);
    
    // 4. Управление сервоприводом
    controlServo(sail_angle);
    
    // 5. Вывод информации (с ограничением частоты)
    if (millis() - lastPrint >= PRINT_INTERVAL) {
        printInfo();
        lastPrint = millis();
    }
    
    // 6. Небольшая задержка для стабильности АЦП
    delay(10);
}

// ============================================
// ДОПОЛНИТЕЛЬНЫЕ ФУНКЦИИ
// ============================================

// Тест калибровки АЦП
void testADC() {
    Serial.println("\n=== ТЕСТ АЦП ESP32 ===");
    Serial.println("Покрутите потенциометры для проверки:");
    
    for (int i = 0; i < 10; i++) {
        int raw1 = analogRead(PIN_POT_KURS);
        int raw2 = analogRead(PIN_POT_VETER);
        float volt1 = (raw1 / ADC_MAX) * VOLTAGE_MAX;
        float volt2 = (raw2 / ADC_MAX) * VOLTAGE_MAX;
        
        Serial.printf("ADC1: %4d (%.2fВ)  ADC2: %4d (%.2fВ)\n", 
                     raw1, volt1, raw2, volt2);
        delay(500);
    }
}

// Калибровка нуля (вызывать при установке)
void calibrateZero() {
    Serial.println("\n=== КАЛИБРОВКА НУЛЯ ===");
    Serial.println("Установите курс на север (0°)");
    Serial.println("и направление ветра на север (X=180)");
    Serial.println("Нажмите любую клавишу для продолжения...");
    
    while (!Serial.available()) {
        delay(100);
    }
    Serial.read(); // Очистка буфера
    
    // Здесь можно сохранить калибровочные значения в EEPROM
    Serial.println("Калибровка завершена!");
}