// =====================================
// ИНТЕРАКТИВНАЯ НАСТРОЙКА PID
// =====================================

class InteractivePIDTuner {
private:
  float kp, ki, kd;
  float integral, lastError;
  float setpoint, measurement;
  
  // История для анализа
  float errorHistory[100];
  float outputHistory[100];
  int historyIndex = 0;
  
public:
  InteractivePIDTuner() : kp(1.0), ki(0.1), kd(0.05) {}
  
  float update(float target, float current, float dt) {
    setpoint = target;
    measurement = current;
    
    float error = target - current;
    
    // P составляющая
    float proportional = kp * error;
    
    // I составляющая
    integral += error * dt;
    integral = constrain(integral, -10, 10);  // Anti-windup
    float integralTerm = ki * integral;
    
    // D составляющая  
    float derivative = (error - lastError) / dt;
    float derivativeTerm = kd * derivative;
    lastError = error;
    
    float output = proportional + integralTerm + derivativeTerm;
    
    // Сохранение истории для анализа
    errorHistory[historyIndex] = error;
    outputHistory[historyIndex] = output;
    historyIndex = (historyIndex + 1) % 100;
    
    return constrain(output, -1.0, 1.0);
  }
  
  void tunePID() {
    Serial.println("=== ИНТЕРАКТИВНАЯ НАСТРОЙКА PID ===");
    Serial.println("Команды:");
    Serial.println("p+/p- : увеличить/уменьшить Kp");
    Serial.println("i+/i- : увеличить/уменьшить Ki");  
    Serial.println("d+/d- : увеличить/уменьшить Kd");
    Serial.println("r     : сброс интегратора");
    Serial.println("s     : показать статистику");
    Serial.println("a     : автонастройка");
    
    while (true) {
      if (Serial.available()) {
        String command = Serial.readString();
        command.trim();
        
        if (command == "p+") kp += 0.1;
        else if (command == "p-") kp -= 0.1;
        else if (command == "i+") ki += 0.01;
        else if (command == "i-") ki -= 0.01;
        else if (command == "d+") kd += 0.01;
        else if (command == "d-") kd -= 0.01;
        else if (command == "r") integral = 0;
        else if (command == "s") showStatistics();
        else if (command == "a") autoTune();
        else if (command == "exit") break;
        
        kp = max(0, kp);
        ki = max(0, ki);
        kd = max(0, kd);
        
        Serial.printf("PID: P=%.2f I=%.3f D=%.3f\n", kp, ki, kd);
      }
      
      // Симуляция системы для настройки
      static float simulatedPosition = 0;
      static float simulatedVelocity = 0;
      float targetPosition = 10.0 * sin(millis() / 3000.0);  // Синусоида
      
      float pidOutput = update(targetPosition, simulatedPosition, 0.02);
      
      // Простая модель системы
      simulatedVelocity += pidOutput * 0.02;
      simulatedVelocity *= 0.95;  // Трение
      simulatedPosition += simulatedVelocity * 0.02;
      
      Serial.printf("Target: %6.2f | Current: %6.2f | Error: %6.2f | Output: %6.2f\n",
        targetPosition, simulatedPosition, targetPosition - simulatedPosition, pidOutput);
      
      delay(20);
    }
  }
  
  void showStatistics() {
    // Анализ производительности
    float avgError = 0, maxError = 0, oscillations = 0;
    
    for (int i = 0; i < 100; i++) {
      avgError += abs(errorHistory[i]);
      maxError = max(maxError, abs(errorHistory[i]));
    }
    avgError /= 100;
    
    // Подсчет колебаний
    for (int i = 1; i < 99; i++) {
      if ((outputHistory[i-1] < outputHistory[i]) && (outputHistory[i] > outputHistory[i+1])) {
        oscillations++;
      }
    }
    
    Serial.println("\n=== СТАТИСТИКА ПРОИЗВОДИТЕЛЬНОСТИ ===");
    Serial.printf("Средняя ошибка: %.3f\n", avgError);
    Serial.printf("Максимальная ошибка: %.3f\n", maxError);
    Serial.printf("Колебания за 100 семплов: %.0f\n", oscillations);
    
    // Рекомендации
    if (avgError > 1.0) {
      Serial.println("💡 Большая ошибка - увеличьте Kp");
    }
    if (oscillations > 20) {
      Serial.println("💡 Много колебаний - уменьшите Kp или увеличьте Kd");  
    }
    if (maxError > 0.5 && oscillations < 5) {
      Serial.println("💡 Медленная реакция - увеличьте Kp или уменьшите Kd");
    }
  }
  
  void autoTune() {
    Serial.println("Автонастройка PID по методу Ziegler-Nichols...");
    
    // Упрощенная автонастройка
    float bestKp = 1.0;
    float bestPerformance = 999;
    
    // Поиск критического Kp
    for (float testKp = 0.1; testKp <= 5.0; testKp += 0.1) {
      kp = testKp;
      ki = 0;
      kd = 0;
      
      float performance = testPerformance(3.0);  // Тест 3 секунды
      
      if (performance < bestPerformance) {
        bestPerformance = performance;
        bestKp = testKp;
      }
      
      Serial.printf("Тест Kp=%.1f, производительность: %.2f\n", testKp, performance);
    }
    
    // Установка оптимальных параметров по Ziegler-Nichols
    kp = bestKp * 0.6;
    ki = 2.0 * kp / 1.0;  // Ti = 1 секунда
    kd = kp * 0.125;      // Td = 0.125 секунды
    
    Serial.printf("Автонастройка завершена: P=%.2f I=%.3f D=%.3f\n", kp, ki, kd);
  }
  
private:
  float testPerformance(float testDuration) {
    // Тестируем производительность PID за заданное время
    float totalError = 0;
    int samples = testDuration * 50;  // 50Hz
    
    float testPosition = 0, testVelocity = 0;
    integral = 0;
    lastError = 0;
    
    for (int i = 0; i < samples; i++) {
      float target = 5.0;  // Ступенчатое воздействие
      float pidOut = update(target, testPosition, 0.02);
      
      testVelocity += pidOut * 0.02;
      testVelocity *= 0.95;
      testPosition += testVelocity * 0.02;
      
      totalError += abs(target - testPosition);
    }
    
    return totalError / samples;
  }
};

// Пример использования
InteractivePIDTuner pidTuner;

void setupPIDTuning() {
  pidTuner.tunePID();
}

// =====================================
// КАСКАДНАЯ СИСТЕМА УПРАВЛЕНИЯ
// =====================================

class CascadeController {
public:
  struct Position {
    float x, y, z;
  };
  
  struct Attitude {
    float roll, pitch, yaw;
  };
  
  struct Rates {
    float rollRate, pitchRate, yawRate;
  };
  
private:
  // PID контроллеры для каждого уровня
  InteractivePIDTuner positionXPID, positionYPID, positionZPID;
  InteractivePIDTuner attitudeRollPID, attitudePitchPID, attitudeYawPID;
  InteractivePIDTuner rateRollPID, ratePitchPID, rateYawPID;
  
  // Ограничения между уровнями
  float maxTiltAngle = 30.0;      // Максимальный наклон ±30°
  float maxYawRate = 180.0;       // Максимальная скорость поворота ±180°/с
  float maxRotationRate = 720.0;  // Максимальная угловая скорость ±720°/с
  
public:
  CascadeController() {
    // Настройка PID параметров для каждого уровня
    setupPositionControllers();
    setupAttitudeControllers(); 
    setupRateControllers();
  }
  
  // Главная функция управления
  void update(Position desiredPos, Position currentPos,
              Attitude currentAtt, Rates currentRates,
              float dt, float motorOutputs[4]) {
    
    // УРОВЕНЬ 1: Управление позицией → желаемые углы
    Attitude desiredAttitude = positionControl(desiredPos, currentPos, dt);
    
    // УРОВЕНЬ 2: Управление углами → желаемые угловые скорости  
    Rates desiredRates = attitudeControl(desiredAttitude, currentAtt, dt);
    
    // УРОВЕНЬ 3: Управление угловыми скоростями → команды моторам
    rateControl(desiredRates, currentRates, dt, motorOutputs);
  }
  
private:
  void setupPositionControllers() {
    // Позиционные контроллеры - медленные, интегральные
    // Настройки для плавного полета
  }
  
  void setupAttitudeControllers() {
    // Угловые контроллеры - средняя скорость
    // Баланс между точностью и стабильностью
  }
  
  void setupRateControllers() {
    // Скоростные контроллеры - быстрые, без интегральной составляющей
    // Максимальная отзывчивость
  }
  
  Attitude positionControl(Position desired, Position current, float dt) {
    Attitude output;
    
    // X позиция → Roll угол
    float xError = desired.x - current.x;
    float rollCommand = positionXPID.update(0, xError, dt);
    output.roll = constrain(rollCommand, -maxTiltAngle, maxTiltAngle);
    
    // Y позиция → Pitch угол  
    float yError = desired.y - current.y;
    float pitchCommand = positionYPID.update(0, yError, dt);
    output.pitch = constrain(pitchCommand, -maxTiltAngle, maxTiltAngle);
    
    // Z позиция → Throttle (обрабатывается отдельно)
    // Yaw остается от пилота или навигации
    output.yaw = 0;  // Или from external source
    
    return output;
  }
  
  Rates attitudeControl(Attitude desired, Attitude current, float dt) {
    Rates output;
    
    // Roll угол → Roll rate
    float rollError = desired.roll - current.roll;
    output.rollRate = attitudeRollPID.update(0, rollError, dt);
    output.rollRate = constrain(output.rollRate, -maxRotationRate, maxRotationRate);
    
    // Pitch угол → Pitch rate
    float pitchError = desired.pitch - current.pitch;
    output.pitchRate = attitudePitchPID.update(0, pitchError, dt);
    output.pitchRate = constrain(output.pitchRate, -maxRotationRate, maxRotationRate);
    
    // Yaw угол → Yaw rate
    float yawError = desired.yaw - current.yaw;
    // Обработка перехода через ±180°
    if (yawError > 180) yawError -= 360;
    if (yawError < -180) yawError += 360;
    
    output.yawRate = attitudeYawPID.update(0, yawError, dt);
    output.yawRate = constrain(output.yawRate, -maxYawRate, maxYawRate);
    
    return output;
  }
  
  void rateControl(Rates desired, Rates current, float dt, float motors[4]) {
    // Контроль угловых скоростей → PWM моторов
    
    float rollOutput = rateRollPID.update(desired.rollRate, current.rollRate, dt);
    float pitchOutput = ratePitchPID.update(desired.pitchRate, current.pitchRate, dt);
    float yawOutput = rateYawPID.update(desired.yawRate, current.yawRate, dt);
    
    // Микширование на квадрокоптер (X конфигурация)
    float throttle = 0.5;  // Базовый газ (от altitude controller)
    
    motors[0] = throttle + pitchOutput - rollOutput - yawOutput;  // Front Right
    motors[1] = throttle - pitchOutput - rollOutput + yawOutput;  // Rear Right
    motors[2] = throttle - pitchOutput + rollOutput - yawOutput;  // Rear Left  
    motors[3] = throttle + pitchOutput + rollOutput + yawOutput;  // Front Left
    
    // Нормализация (если сумма превышает максимум)
    float maxMotor = max({motors[0], motors[1], motors[2], motors[3]});
    if (maxMotor > 1.0) {
      float scale = 1.0 / maxMotor;
      for (int i = 0; i < 4; i++) {
        motors[i] *= scale;
      }
    }
    
    // Ограничение минимума
    for (int i = 0; i < 4; i++) {
      motors[i] = constrain(motors[i], 0.0, 1.0);
    }
  }
};

// Пример интеграции с системой
CascadeController flightController;

void flightControlLoop() {
  // Получение данных датчиков
  CascadeController::Position currentPos = getCurrentPosition();
  CascadeController::Attitude currentAtt = getCurrentAttitude();
  CascadeController::Rates currentRates = getCurrentRates();
  
  // Желаемая позиция (от навигации или пилота)
  CascadeController::Position targetPos = getTargetPosition();
  
  // Управление
  float motorCommands[4];
  flightController.update(targetPos, currentPos, currentAtt, currentRates, 0.01, motorCommands);
  
  // Вывод на моторы
  setMotorOutputs(motorCommands);
}

// =====================================
// ДИСПЕТЧЕР РЕЖИМОВ ПОЛЕТА
// =====================================

class FlightModeManager {
public:
  enum FlightMode {
    DISARMED = 0,
    ACRO = 1,
    STABILIZE = 2,
    ALT_HOLD = 3,
    LOITER = 4,
    GUIDED = 5,
    AUTO = 6,
    RTL = 7,
    LAND = 8,
    FAILSAFE = 9
  };
  
  struct ControlState {
    // Входы от пилота
    float rollInput, pitchInput, yawInput, throttleInput;
    
    // Навигационные команды
    float targetLat, targetLon, targetAlt;
    float targetVelX, targetVelY, targetVelZ;
    
    // Состояние системы
    bool isArmed;
    bool gpsHealthy;
    bool altitudeHealthy;
    bool radioHealthy;
    
    // Выходы для контроллера
    float rollCommand, pitchCommand, yawCommand, throttleCommand;
    bool autoControlActive;
  };
  
private:
  FlightMode currentMode = DISARMED;
  FlightMode previousMode = DISARMED;
  ControlState state;
  
  // Для режимов с памятью
  float altHoldTarget = 0;
  float loiterLat = 0, loiterLon = 0;
  bool targetSet = false;
  
  // Таймеры и безопасность
  unsigned long modeChangeTime = 0;
  unsigned long lastRadioUpdate = 0;
  
public:
  bool setMode(FlightMode newMode) {
    // Проверка возможности перехода
    if (!canTransitionTo(newMode)) {
      Serial.printf("Cannot transition from %s to %s\n", 
        getModeString(currentMode), getModeString(newMode));
      return false;
    }
    
    // Выход из текущего режима
    exitMode(currentMode);
    
    // Смена режима
    previousMode = currentMode;
    currentMode = newMode;
    modeChangeTime = millis();
    
    // Вход в новый режим
    enterMode(newMode);
    
    Serial.printf("Mode changed: %s → %s\n", 
      getModeString(previousMode), getModeString(currentMode));
    
    return true;
  }
  
  void update(ControlState& inputState) {
    state = inputState;
    
    // Проверка безопасности
    checkSafetyConditions();
    
    // Обработка текущего режима
    switch (currentMode) {
      case DISARMED:
        handleDisarmed();
        break;
      case ACRO:
        handleAcro();
        break;
      case STABILIZE:
        handleStabilize();
        break;
      case ALT_HOLD:
        handleAltHold();
        break;
      case LOITER:
        handleLoiter();
        break;
      case GUIDED:
        handleGuided();
        break;
      case AUTO:
        handleAuto();
        break;
      case RTL:
        handleRTL();
        break;
      case LAND:
        handleLand();
        break;
      case FAILSAFE:
        handleFailsafe();
        break;
    }
    
    // Обновление выходного состояния
    inputState = state;
  }
  
  FlightMode getCurrentMode() { return currentMode; }
  String getModeString() { return getModeString(currentMode); }
  
private:
  bool canTransitionTo(FlightMode newMode) {
    switch (newMode) {
      case DISARMED:
        return true;  // Всегда можно disarm
        
      case ACRO:
      case STABILIZE:
        return state.isArmed && state.radioHealthy;
        
      case ALT_HOLD:
        return state.isArmed && state.radioHealthy && state.altitudeHealthy;
        
      case LOITER:
      case GUIDED:
      case AUTO:
      case RTL:
        return state.isArmed && state.radioHealthy && 
               state.altitudeHealthy && state.gpsHealthy;
        
      case LAND:
        return state.isArmed;
        
      case FAILSAFE:
        return true;  // Failsafe можно всегда
    }
    return false;
  }
  
  void enterMode(FlightMode mode) {
    switch (mode) {
      case ALT_HOLD:
        altHoldTarget = getCurrentAltitude();
        targetSet = true;
        Serial.printf("Altitude hold target set: %.1fm\n", altHoldTarget);
        break;
        
      case LOITER:
        if (state.gpsHealthy) {
          loiterLat = getCurrentLatitude();
          loiterLon = getCurrentLongitude();
          altHoldTarget = getCurrentAltitude();
          targetSet = true;
          Serial.printf("Loiter target: %.6f, %.6f, %.1fm\n", 
            loiterLat, loiterLon, altHoldTarget);
        }
        break;
        
      case RTL:
        state.targetLat = getHomeLat();
        state.targetLon = getHomeLon();
        state.targetAlt = getHomeAlt() + 20.0;  // +20m безопасная высота
        targetSet = true;
        Serial.println("Return to Launch initiated");
        break;
    }
  }
  
  void exitMode(FlightMode mode) {
    // Очистка состояния при выходе из режима
    targetSet = false;
    state.autoControlActive = false;
  }
  
  void handleDisarmed() {
    // В режиме DISARMED все моторы выключены
    state.rollCommand = 0;
    state.pitchCommand = 0;
    state.yawCommand = 0;
    state.throttleCommand = 0;
    state.autoControlActive = false;
  }
  
  void handleAcro() {
    // Прямое управление угловыми скоростями
    state.rollCommand = state.rollInput * 720.0;      // ±720°/s
    state.pitchCommand = state.pitchInput * 720.0;
    state.yawCommand = state.yawInput * 360.0;        // ±360°/s
    state.throttleCommand = state.throttleInput;
    state.autoControlActive = false;
  }
  
  void handleStabilize() {
    // Стабилизация углов
    state.rollCommand = state.rollInput * 45.0;       // ±45° max angle
    state.pitchCommand = state.pitchInput * 45.0;
    state.yawCommand = state.yawInput * 180.0;        // ±180°/s yaw rate
    state.throttleCommand = state.throttleInput;
    state.autoControlActive = false;
  }
  
  void handleAltHold() {
    // Стабилизация + удержание высоты
    handleStabilize();  // Базовая стабилизация
    
    // Автоматическое управление высотой
    if (targetSet) {
      // Изменение целевой высоты стиком газа
      if (abs(state.throttleInput - 0.5) > 0.1) {
        altHoldTarget += (state.throttleInput - 0.5) * 2.0;  // 2 м/с max
        Serial.printf("New altitude target: %.1fm\n", altHoldTarget);
      }
      
      state.targetAlt = altHoldTarget;
      state.autoControlActive = true;
    }
  }
  
  void handleLoiter() {
    // Удержание позиции GPS + высоты
    if (targetSet && state.gpsHealthy) {
      state.targetLat = loiterLat;
      state.targetLon = loiterLon;
      state.targetAlt = altHoldTarget;
      
      // Пилот может сместить target стиками
      if (abs(state.rollInput) > 0.1 || abs(state.pitchInput) > 0.1) {
        // Переключаемся в режим "nudge" - смещение позиции
        float nudgeDistance = 0.00001;  // ~1 метр в градусах
        loiterLat += state.pitchInput * nudgeDistance;
        loiterLon += state.rollInput * nudgeDistance;
      }
      
      state.autoControlActive = true;
    } else {
      // Fallback к ALT_HOLD если GPS потерян
      setMode(ALT_HOLD);
    }
  }
  
  void handleGuided() {
    // Полет к заданной точке
    if (targetSet) {
      // Команды устанавливаются извне (GCS, companion computer)
      state.autoControlActive = true;
    }
  }
  
  void handleAuto() {
    // Выполнение автономной миссии
    // Здесь должна быть интеграция с mission planner
    state.autoControlActive = true;
    
    // Простая заглушка - лететь по квадрату
    executeSimpleMission();
  }
  
  void handleRTL() {
    // Возврат домой
    if (!targetSet) {
      enterMode(RTL);  // Переустановка target если потерян
    }
    
    float distanceToHome = calculateDistance(
      getCurrentLatitude(), getCurrentLongitude(),
      state.targetLat, state.targetLon);
    
    if (distanceToHome > 5.0) {
      // Лететь домой
      state.autoControlActive = true;
    } else {
      // Достигли дома - переключаемся на посадку
      setMode(LAND);
    }
  }
  
  void handleLand() {
    // Автоматическая посадка
    state.targetVelZ = -1.0;  // Снижение 1 м/с
    state.autoControlActive = true;
    
    // Проверка касания земли
    if (getCurrentAltitude() < 0.5 && getCurrentVerticalSpeed() < 0.1) {
      Serial.println("Landing detected - disarming");
      setMode(DISARMED);
    }
  }
  
  void handleFailsafe() {
    // Аварийные процедуры
    Serial.println("FAILSAFE MODE - Emergency procedures");
    
    static unsigned long failsafeStartTime = 0;
    if (failsafeStartTime == 0) {
      failsafeStartTime = millis();
    }
    
    unsigned long failsafeTime = millis() - failsafeStartTime;
    
    if (failsafeTime < 5000) {
      // Первые 5 секунд - попытка стабилизации
      state.rollCommand = 0;
      state.pitchCommand = 0;
      state.yawCommand = 0;
      state.throttleCommand = 0.4;  // Низкий газ
    } else {
      // После 5 секунд - аварийная посадка
      setMode(LAND);
      failsafeStartTime = 0;
    }
    
    state.autoControlActive = true;
  }
  
  void checkSafetyConditions() {
    // Проверка потери радиосвязи
    if (millis() - lastRadioUpdate > 2000) {  // 2 секунды без сигнала
      state.radioHealthy = false;
      if (currentMode != FAILSAFE && currentMode != DISARMED) {
        Serial.println("Radio failsafe triggered!");
        setMode(FAILSAFE);
      }
    }
    
    // Проверка низкого заряда батареи
    float batteryVoltage = getBatteryVoltage();
    if (batteryVoltage < 10.5 && state.isArmed) {  // Критично для 3S LiPo
      Serial.println("Low battery failsafe!");
      setMode(RTL);
    }
    
    // Проверка потери GPS в навигационных режимах
    if (!state.gpsHealthy && 
        (currentMode == LOITER || currentMode == GUIDED || 
         currentMode == AUTO || currentMode == RTL)) {
      Serial.println("GPS failsafe - switching to ALT_HOLD");
      setMode(ALT_HOLD);
    }
  }
  
  void executeSimpleMission() {
    // Простая миссия - полет по квадрату
    static int waypointIndex = 0;
    static float waypoints[][2] = {
      {0.0001, 0.0001},   // +10m North, +10m East
      {0.0001, -0.0001},  // +10m North, -10m East  
      {-0.0001, -0.0001}, // -10m North, -10m East
      {-0.0001, 0.0001}   // -10m North, +10m East
    };
    
    float homeLat = getHomeLat();
    float homeLon = getHomeLon();
    
    state.targetLat = homeLat + waypoints[waypointIndex][0];
    state.targetLon = homeLon + waypoints[waypointIndex][1];
    state.targetAlt = getHomeAlt() + 10.0;
    
    // Проверка достижения waypoint
    float distance = calculateDistance(
      getCurrentLatitude(), getCurrentLongitude(),
      state.targetLat, state.targetLon);
    
    if (distance < 2.0) {  // Достигли waypoint с точностью 2м
      waypointIndex = (waypointIndex + 1) % 4;
      Serial.printf("Waypoint %d reached, proceeding to waypoint %d\n", 
        waypointIndex-1, waypointIndex);
    }
  }
  
  // Вспомогательные функции
  String getModeString(FlightMode mode) {
    switch (mode) {
      case DISARMED: return "DISARMED";
      case ACRO: return "ACRO";
      case STABILIZE: return "STABILIZE";
      case ALT_HOLD: return "ALT_HOLD";
      case LOITER: return "LOITER";
      case GUIDED: return "GUIDED";
      case AUTO: return "AUTO";
      case RTL: return "RTL";
      case LAND: return "LAND";
      case FAILSAFE: return "FAILSAFE";
      default: return "UNKNOWN";
    }
  }
  
  float calculateDistance(float lat1, float lon1, float lat2, float lon2) {
    // Упрощенный расчет расстояния (для небольших дистанций)
    float dlat = lat2 - lat1;
    float dlon = lon2 - lon1;
    return sqrt(dlat*dlat + dlon*dlon) * 111319.5;  // Примерно в метрах
  }
  
  // Заглушки для получения данных датчиков
  float getCurrentAltitude() { return 10.0; }  // Получить из барометра
  float getCurrentLatitude() { return 0.0; }   // Получить из GPS
  float getCurrentLongitude() { return 0.0; }  // Получить из GPS
  float getCurrentVerticalSpeed() { return 0.0; }
  float getBatteryVoltage() { return 12.0; }
  float getHomeLat() { return 0.0; }
  float getHomeLon() { return 0.0; }
  float getHomeAlt() { return 0.0; }
};

// Интеграция в основной цикл
FlightModeManager flightModes;

void flightLoop() {
  // Получение входных данных
  FlightModeManager::ControlState controlState;
  controlState.rollInput = getRollInput();
  controlState.pitchInput = getPitchInput();
  controlState.yawInput = getYawInput();
  controlState.throttleInput = getThrottleInput();
  
  controlState.isArmed = getArmStatus();
  controlState.gpsHealthy = getGPSStatus();
  controlState.altitudeHealthy = getAltitudeStatus();
  controlState.radioHealthy = getRadioStatus();
  
  // Обработка режимов
  flightModes.update(controlState);
  
  // Использование результатов
  if (controlState.autoControlActive) {
    // Передача в автономный контроллер
    setAutonomousTarget(controlState.targetLat, controlState.targetLon, controlState.targetAlt);
  } else {
    // Передача в ручной контроллер
    setManualCommands(controlState.rollCommand, controlState.pitchCommand, 
                     controlState.yawCommand, controlState.throttleCommand);
  }
}

// =====================================
// СИСТЕМА НАВИГАЦИИ ПО WAYPOINTS
// =====================================

class WaypointNavigator {
public:
  struct Waypoint {
    double latitude, longitude;
    float altitude;
    float speed;              // Желаемая скорость м/с
    float acceptanceRadius;   // Радиус принятия waypoint
    int waitTime;            // Время ожидания в секундах
    int action;              // Действие в точке (фото, сброс груза и т.д.)
  };
  
  struct NavigationState {
    double currentLat, currentLon;
    float currentAlt;
    float currentSpeed;
    float bearing;           // Направление к цели
    float distance;          // Расстояние до цели
    float crossTrackError;   // Отклонение от линии пути
  };
  
private:
  std::vector<Waypoint> mission;
  int currentWaypointIndex = 0;
  bool missionActive = false;
  unsigned long waypointReachTime = 0;
  
  NavigationState navState;
  
public:
  void loadMission(const std::vector<Waypoint>& waypoints) {
    mission = waypoints;
    currentWaypointIndex = 0;
    missionActive = false;
    
    Serial.printf("Mission loaded: %d waypoints\n", mission.size());
    for (int i = 0; i < mission.size(); i++) {
      Serial.printf("WP%d: %.6f, %.6f, %.1fm\n", 
        i, mission[i].latitude, mission[i].longitude, mission[i].altitude);
    }
  }
  
  void startMission() {
    if (mission.empty()) {
      Serial.println("No mission loaded!");
      return;
    }
    
    missionActive = true;
    currentWaypointIndex = 0;
    waypointReachTime = 0;
    
    Serial.println("Mission started!");
    printCurrentWaypoint();
  }
  
  void stopMission() {
    missionActive = false;
    Serial.println("Mission stopped");
  }
  
  bool update(double currentLat, double currentLon, float currentAlt) {
    if (!missionActive || mission.empty()) {
      return false;
    }
    
    navState.currentLat = currentLat;
    navState.currentLon = currentLon;
    navState.currentAlt = currentAlt;
    
    if (currentWaypointIndex >= mission.size()) {
      // Миссия завершена
      missionActive = false;
      Serial.println("🎉 Mission completed!");
      return false;
    }
    
    Waypoint& currentWP = mission[currentWaypointIndex];
    
    // Расчет навигационных параметров
    calculateNavigation(currentWP);
    
    // Проверка достижения waypoint
    if (navState.distance <= currentWP.acceptanceRadius) {
      if (waypointReachTime == 0) {
        waypointReachTime = millis();
        Serial.printf("Waypoint %d reached! Distance: %.1fm\n", 
          currentWaypointIndex, navState.distance);
        
        // Выполнение действия в точке
        executeWaypointAction(currentWP.action);
      }
      
      // Ожидание в точке
      if (millis() - waypointReachTime >= currentWP.waitTime * 1000) {
        currentWaypointIndex++;
        waypointReachTime = 0;
        
        if (currentWaypointIndex < mission.size()) {
          Serial.printf("Proceeding to waypoint %d\n", currentWaypointIndex);
          printCurrentWaypoint();
        }
      }
    }
    
    return true;
  }
  
  NavigationState getNavigationState() { return navState; }
  
  // Генерация стандартных миссий
  void generateSurveyMission(double centerLat, double centerLon, 
                           float width, float height, float spacing, float altitude) {
    mission.clear();
    
    // Сетка для съемки
    int lines = height / spacing;
    bool leftToRight = true;
    
    for (int i = 0; i < lines; i++) {
      float y = -height/2 + i * spacing;
      
      if (leftToRight) {
        // Слева направо
        addWaypoint(centerLat + y/111319.5, centerLon - width/2/111319.5, altitude);
        addWaypoint(centerLat + y/111319.5, centerLon + width/2/111319.5, altitude);
      } else {
        // Справа налево
        addWaypoint(centerLat + y/111319.5, centerLon + width/2/111319.5, altitude);
        addWaypoint(centerLat + y/111319.5, centerLon - width/2/111319.5, altitude);
      }
      
      leftToRight = !leftToRight;
    }
    
    Serial.printf("Survey mission generated: %d waypoints\n", mission.size());
  }
  
  void generateCircleMission(double centerLat, double centerLon, 
                           float radius, int points, float altitude) {
    mission.clear();
    
    for (int i = 0; i <= points; i++) {
      float angle = i * 2 * PI / points;
      float lat = centerLat + (radius * cos(angle)) / 111319.5;
      float lon = centerLon + (radius * sin(angle)) / 111319.5;
      
      addWaypoint(lat, lon, altitude);
    }
    
    Serial.printf("Circle mission generated: %d waypoints\n", mission.size());
  }
  
private:
  void addWaypoint(double lat, double lon, float alt, 
                  float speed = 5.0, float radius = 3.0, int wait = 0, int action = 0) {
    Waypoint wp;
    wp.latitude = lat;
    wp.longitude = lon;
    wp.altitude = alt;
    wp.speed = speed;
    wp.acceptanceRadius = radius;
    wp.waitTime = wait;
    wp.action = action;
    
    mission.push_back(wp);
  }
  
  void calculateNavigation(const Waypoint& target) {
    // Расстояние до цели
    navState.distance = calculateDistance(
      navState.currentLat, navState.currentLon,
      target.latitude, target.longitude);
    
    // Направление к цели
    navState.bearing = calculateBearing(
      navState.currentLat, navState.currentLon,
      target.latitude, target.longitude);
    
    // Cross-track error (отклонение от линии пути)
    if (currentWaypointIndex > 0) {
      Waypoint& prevWP = mission[currentWaypointIndex - 1];
      navState.crossTrackError = calculateCrossTrackError(
        prevWP.latitude, prevWP.longitude,
        target.latitude, target.longitude,
        navState.currentLat, navState.currentLon);
    } else {
      navState.crossTrackError = 0;
    }
  }
  
  float calculateDistance(double lat1, double lon1, double lat2, double lon2) {
    // Haversine formula для точного расчета
    double R = 6371000; // Радиус Земли в метрах
    double phi1 = lat1 * PI / 180.0;
    double phi2 = lat2 * PI / 180.0;
    double deltaPhi = (lat2 - lat1) * PI / 180.0;
    double deltaLambda = (lon2 - lon1) * PI / 180.0;
    
    double a = sin(deltaPhi/2) * sin(deltaPhi/2) +
               cos(phi1) * cos(phi2) *
               sin(deltaLambda/2) * sin(deltaLambda/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    
    return R * c;
  }
  
  float calculateBearing(double lat1, double lon1, double lat2, double lon2) {
    double phi1 = lat1 * PI / 180.0;
    double phi2 = lat2 * PI / 180.0;
    double deltaLambda = (lon2 - lon1) * PI / 180.0;
    
    double y = sin(deltaLambda) * cos(phi2);
    double x = cos(phi1) * sin(phi2) - sin(phi1) * cos(phi2) * cos(deltaLambda);
    
    double bearing = atan2(y, x) * 180.0 / PI;
    return fmod(bearing + 360.0, 360.0);  // Нормализация 0-360°
  }
  
  float calculateCrossTrackError(double lat1, double lon1, double lat2, double lon2,
                               double currentLat, double currentLon) {
    // Упрощенный расчет для небольших расстояний
    double d13 = calculateDistance(lat1, lon1, currentLat, currentLon);
    double bearing13 = calculateBearing(lat1, lon1, currentLat, currentLon) * PI / 180.0;
    double bearing12 = calculateBearing(lat1, lon1, lat2, lon2) * PI / 180.0;
    
    return asin(sin(d13/6371000) * sin(bearing13 - bearing12)) * 6371000;
  }
  
  void executeWaypointAction(int action) {
    switch (action) {
      case 1:
        Serial.println("📸 Taking photo");
        // triggerCamera();
        break;
      case 2:
        Serial.println("📦 Dropping payload");
        // dropPayload();
        break;
      case 3:
        Serial.println("📡 Recording telemetry");
        // recordTelemetry();
        break;
      default:
        // Никакого действия
        break;
    }
  }
  
  void printCurrentWaypoint() {
    if (currentWaypointIndex < mission.size()) {
      Waypoint& wp = mission[currentWaypointIndex];
      Serial.printf("Current target - WP%d: %.6f, %.6f, %.1fm\n",
        currentWaypointIndex, wp.latitude, wp.longitude, wp.altitude);
    }
  }
};

// Интеграция с системой управления
WaypointNavigator navigator;

void missionLoop() {
  // Получение текущей позиции
  double currentLat = getCurrentLatitude();
  double currentLon = getCurrentLongitude();
  float currentAlt = getCurrentAltitude();
  
  // Обновление навигации
  if (navigator.update(currentLat, currentLon, currentAlt)) {
    WaypointNavigator::NavigationState nav = navigator.getNavigationState();
    
    // Передача команд в систему управления
    setNavigationTarget(nav.bearing, nav.distance, nav.crossTrackError);
    
    // Отладочная информация
    Serial.printf("Nav: bearing=%.1f° distance=%.1fm crosstrack=%.1fm\n",
      nav.bearing, nav.distance, nav.crossTrackError);
  }
}

// =====================================
// КОМПЛЕКСНАЯ СИСТЕМА ТЕЛЕМЕТРИИ
// =====================================

#include <WiFi.h>
#include <WebServer.h>
#include <WebSocketsServer.h>
#include <ArduinoJson.h>
#include <SPIFFS.h>

class TelemetrySystem {
public:
  struct FlightData {
    // Временная метка
    unsigned long timestamp;
    
    // Ориентация и движение
    float roll, pitch, yaw;
    float rollRate, pitchRate, yawRate;
    float accelX, accelY, accelZ;
    
    // Позиция и навигация
    double latitude, longitude;
    float altitude, altitudeGPS;
    float velocityN, velocityE, velocityD;
    float groundSpeed, airSpeed;
    
    // Система управления
    float motor1, motor2, motor3, motor4;
    float rollCommand, pitchCommand, yawCommand, throttleCommand;
    int flightMode;
    bool armed;
    
    // Система питания
    float batteryVoltage, batteryCurrent;
    float batteryCapacityUsed, batteryRemaining;
    int batteryCells;
    
    // Датчики окружения
    float temperature, pressure, humidity;
    float windSpeed, windDirection;
    
    // Системные данные
    float cpuUsage, memoryUsage;
    int wifiSignal, gpsStatus;
    int loopFrequency;
    
    // Статус и ошибки
    uint32_t statusFlags;
    uint32_t errorFlags;
  };
  
private:
  WebServer httpServer;
  WebSocketsServer webSocketServer;
  
  FlightData currentData;
  std::vector<FlightData> dataBuffer;
  static const int BUFFER_SIZE = 1000;
  
  // Настройки передачи
  unsigned long lastTelemetryUpdate = 0;
  unsigned long lastLogWrite = 0;
  int telemetryRate = 10;  // Hz
  int logRate = 50;        // Hz
  
  bool loggingEnabled = true;
  File logFile;
  
public:
  TelemetrySystem() : httpServer(80), webSocketServer(81) {}
  
  void begin() {
    SPIFFS.begin();
    setupWebServer();
    setupWebSocket();
    startLogging();
    
    httpServer.begin();
    webSocketServer.begin();
    
    Serial.println("Telemetry system started");
    Serial.printf("Web interface: http://%s\n", WiFi.localIP().toString().c_str());
  }
  
  void update(const FlightData& data) {
    currentData = data;
    currentData.timestamp = millis();
    
    // Добавление в буфер
    dataBuffer.push_back(currentData);
    if (dataBuffer.size() > BUFFER_SIZE) {
      dataBuffer.erase(dataBuffer.begin());
    }
    
    // Обработка веб-сервера
    httpServer.handleClient();
    webSocketServer.loop();
    
    // Отправка телеметрии
    if (millis() - lastTelemetryUpdate >= 1000 / telemetryRate) {
      sendTelemetryUpdate();
      lastTelemetryUpdate = millis();
    }
    
    // Запись в лог
    if (loggingEnabled && millis() - lastLogWrite >= 1000 / logRate) {
      writeToLog(currentData);
      lastLogWrite = millis();
    }
  }
  
private:
  void setupWebServer() {
    // Главная страница
    httpServer.on("/", [this]() {
      httpServer.send(200, "text/html", getMainPage());
    });
    
    // API для получения текущих данных
    httpServer.on("/api/current", [this]() {
      DynamicJsonDocument doc(2048);
      serializeFlightData(currentData, doc);
      
      String json;
      serializeJson(doc, json);
      httpServer.send(200, "application/json", json);
    });
    
    // API для получения истории
    httpServer.on("/api/history", [this]() {
      DynamicJsonDocument doc(8192);
      JsonArray array = doc.to<JsonArray>();
      
      int startIndex = max(0, (int)dataBuffer.size() - 100);
      for (int i = startIndex; i < dataBuffer.size(); i++) {
        JsonObject obj = array.createNestedObject();
        serializeFlightData(dataBuffer[i], obj);
      }
      
      String json;
      serializeJson(doc, json);
      httpServer.send(200, "application/json", json);
    });
    
    // Скачивание логов
    httpServer.on("/api/download_log", [this]() {
      if (SPIFFS.exists("/flight.log")) {
        File file = SPIFFS.open("/flight.log", "r");
        httpServer.streamFile(file, "application/octet-stream");
        file.close();
      } else {
        httpServer.send(404, "text/plain", "Log file not found");
      }
    });
  }
  
  void setupWebSocket() {
    webSocketServer.onEvent([this](uint8_t num, WStype_t type, uint8_t* payload, size_t length) {
      switch (type) {
        case WStype_DISCONNECTED:
          Serial.printf("WebSocket client %u disconnected\n", num);
          break;
          
        case WStype_CONNECTED:
          Serial.printf("WebSocket client %u connected\n", num);
          break;
          
        case WStype_TEXT:
          processWebSocketCommand(num, (char*)payload);
          break;
      }
    });
  }
  
  void sendTelemetryUpdate() {
    DynamicJsonDocument doc(1024);
    serializeFlightData(currentData, doc);
    
    String json;
    serializeJson(doc, json);
    
    webSocketServer.broadcastTXT(json);
  }
  
  void serializeFlightData(const FlightData& data, JsonDocument& doc) {
    doc["timestamp"] = data.timestamp;
    doc["roll"] = data.roll;
    doc["pitch"] = data.pitch;
    doc["yaw"] = data.yaw;
    doc["altitude"] = data.altitude;
    doc["latitude"] = data.latitude;
    doc["longitude"] = data.longitude;
    doc["battery_voltage"] = data.batteryVoltage;
    doc["battery_current"] = data.batteryCurrent;
    doc["flight_mode"] = data.flightMode;
    doc["armed"] = data.armed;
    doc["motor1"] = data.motor1;
    doc["motor2"] = data.motor2;
    doc["motor3"] = data.motor3;
    doc["motor4"] = data.motor4;
  }
  
  void startLogging() {
    if (loggingEnabled) {
      logFile = SPIFFS.open("/flight.log", "w");
      if (logFile) {
        // Заголовок CSV
        logFile.println("timestamp,roll,pitch,yaw,altitude,lat,lon,battery_v,battery_a,mode,armed,m1,m2,m3,m4");
        Serial.println("Flight logging started");
      }
    }
  }
  
  void writeToLog(const FlightData& data) {
    if (logFile && loggingEnabled) {
      logFile.printf("%lu,%.2f,%.2f,%.2f,%.1f,%.6f,%.6f,%.2f,%.2f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
        data.timestamp, data.roll, data.pitch, data.yaw, data.altitude,
        data.latitude, data.longitude, data.batteryVoltage, data.batteryCurrent,
        data.flightMode, data.armed, data.motor1, data.motor2, data.motor3, data.motor4);
      logFile.flush();
    }
  }
  
  void processWebSocketCommand(uint8_t clientNum, const char* command) {
    DynamicJsonDocument doc(512);
    deserializeJson(doc, command);
    
    String cmd = doc["command"];
    
    if (cmd == "set_telemetry_rate") {
      telemetryRate = doc["rate"];
      Serial.printf("Telemetry rate set to %d Hz\n", telemetryRate);
    }
    else if (cmd == "toggle_logging") {
      loggingEnabled = !loggingEnabled;
      Serial.printf("Logging %s\n", loggingEnabled ? "enabled" : "disabled");
    }
    else if (cmd == "clear_log") {
      if (logFile) {
        logFile.close();
        SPIFFS.remove("/flight.log");
        startLogging();
        Serial.println("Flight log cleared");
      }
    }
  }
  
  String getMainPage() {
    return R"HTML(
<!DOCTYPE html>
<html>
<head>
    <title>Drone Telemetry Dashboard</title>
    <meta charset="utf-8">
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <style>
        body { 
            font-family: 'Segoe UI', Arial, sans-serif; 
            margin: 0; 
            padding: 20px; 
            background: linear-gradient(135deg, #667eea 0%, #764ba2 100%);
            color: white;
        }
        .container { 
            max-width: 1400px; 
            margin: 0 auto; 
        }
        .header {
            text-align: center;
            margin-bottom: 30px;
        }
        .header h1 {
            font-size: 2.5em;
            margin: 0;
            text-shadow: 2px 2px 4px rgba(0,0,0,0.3);
        }
        .status-bar {
            display: flex;
            justify-content: center;
            gap: 20px;
            margin-bottom: 30px;
        }
        .status-item {
            background: rgba(255,255,255,0.2);
            padding: 10px 20px;
            border-radius: 20px;
            backdrop-filter: blur(10px);
        }
        .grid {
            display: grid;
            grid-template-columns: repeat(auto-fit, minmax(300px, 1fr));
            gap: 20px;
            margin-bottom: 30px;
        }
        .card { 
            background: rgba(255,255,255,0.15);
            backdrop-filter: blur(10px);
            padding: 20px; 
            border-radius: 15px; 
            box-shadow: 0 8px 32px rgba(0,0,0,0.1);
            border: 1px solid rgba(255,255,255,0.2);
        }
        .card h3 {
            margin-top: 0;
            font-size: 1.2em;
            border-bottom: 2px solid rgba(255,255,255,0.3);
            padding-bottom: 10px;
        }
        .metric { 
            display: flex;
            justify-content: space-between;
            margin: 10px 0;
            padding: 8px 0;
            border-bottom: 1px solid rgba(255,255,255,0.1);
        }
        .metric-label { 
            font-weight: 500; 
        }
        .metric-value { 
            font-weight: bold; 
            font-family: 'Courier New', monospace;
        }
        .attitude-indicator {
            width: 200px;
            height: 200px;
            margin: 20px auto;
            position: relative;
            border-radius: 50%;
            background: radial-gradient(circle, #87CEEB 0%, #4682B4 100%);
            border: 5px solid rgba(255,255,255,0.3);
        }
        .horizon {
            position: absolute;
            width: 100%;
            height: 50%;
            bottom: 0;
            background: linear-gradient(to bottom, #8B4513 0%, #A0522D 100%);
            border-radius: 0 0 50% 50%;
            transform-origin: center top;
            transition: transform 0.3s ease;
        }
        .aircraft-symbol {
            position: absolute;
            top: 50%;
            left: 50%;
            transform: translate(-50%, -50%);
            width: 60px;
            height: 4px;
            background: #FFD700;
            border-radius: 2px;
        }
        .aircraft-symbol::before {
            content: '';
            position: absolute;
            top: -8px;
            left: 50%;
            transform: translateX(-50%);
            width: 20px;
            height: 4px;
            background: #FFD700;
            border-radius: 2px;
        }
        .controls {
            display: flex;
            gap: 10px;
            margin-top: 20px;
            justify-content: center;
        }
        .btn {
            padding: 10px 20px;
            background: rgba(255,255,255,0.2);
            border: none;
            border-radius: 25px;
            color: white;
            cursor: pointer;
            transition: all 0.3s ease;
            backdrop-filter: blur(10px);
        }
        .btn:hover {
            background: rgba(255,255,255,0.3);
            transform: translateY(-2px);
        }
        .chart-container {
            grid-column: 1 / -1;
            height: 300px;
            background: rgba(255,255,255,0.1);
            border-radius: 15px;
            padding: 20px;
        }
        #chart {
            width: 100%;
            height: 100%;
        }
        .connection-status {
            position: fixed;
            top: 20px;
            right: 20px;
            padding: 10px 15px;
            border-radius: 20px;
            font-weight: bold;
            transition: all 0.3s ease;
        }
        .connected {
            background: rgba(76, 175, 80, 0.9);
        }
        .disconnected {
            background: rgba(244, 67, 54, 0.9);
        }
    </style>
    <script src="https://cdn.jsdelivr.net/npm/chart.js"></script>
</head>
<body>
    <div id="connectionStatus" class="connection-status disconnected">
        ⚫ Disconnected
    </div>
    
    <div class="container">
        <div class="header">
            <h1>🚁 Drone Telemetry Dashboard</h1>
        </div>
        
        <div class="status-bar">
            <div class="status-item">
                <span id="flightMode">DISARMED</span>
            </div>
            <div class="status-item">
                <span id="armedStatus">DISARMED</span>
            </div>
            <div class="status-item">
                GPS: <span id="gpsStatus">No Fix</span>
            </div>
        </div>
        
        <div class="grid">
            <div class="card">
                <h3>🧭 Attitude</h3>
                <div class="attitude-indicator">
                    <div class="horizon" id="horizon"></div>
                    <div class="aircraft-symbol"></div>
                </div>
                <div class="metric">
                    <span class="metric-label">Roll:</span>
                    <span class="metric-value" id="roll">0.0°</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Pitch:</span>
                    <span class="metric-value" id="pitch">0.0°</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Yaw:</span>
                    <span class="metric-value" id="yaw">0.0°</span>
                </div>
            </div>
            
            <div class="card">
                <h3>📍 Position</h3>
                <div class="metric">
                    <span class="metric-label">Latitude:</span>
                    <span class="metric-value" id="latitude">0.000000</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Longitude:</span>
                    <span class="metric-value" id="longitude">0.000000</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Altitude:</span>
                    <span class="metric-value" id="altitude">0.0 m</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Ground Speed:</span>
                    <span class="metric-value" id="groundSpeed">0.0 m/s</span>
                </div>
            </div>
            
            <div class="card">
                <h3>🔋 Power System</h3>
                <div class="metric">
                    <span class="metric-label">Battery Voltage:</span>
                    <span class="metric-value" id="batteryVoltage">0.0 V</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Current Draw:</span>
                    <span class="metric-value" id="batteryCurrent">0.0 A</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Power:</span>
                    <span class="metric-value" id="power">0.0 W</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Remaining:</span>
                    <span class="metric-value" id="batteryRemaining">100%</span>
                </div>
            </div>
            
            <div class="card">
                <h3>🚁 Motors</h3>
                <div class="metric">
                    <span class="metric-label">Motor 1:</span>
                    <span class="metric-value" id="motor1">0%</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Motor 2:</span>
                    <span class="metric-value" id="motor2">0%</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Motor 3:</span>
                    <span class="metric-value" id="motor3">0%</span>
                </div>
                <div class="metric">
                    <span class="metric-label">Motor 4:</span>
                    <span class="metric-value" id="motor4">0%</span>
                </div>
            </div>
            
            <div class="card chart-container">
                <h3>📊 Real-time Data</h3>
                <canvas id="chart"></canvas>
            </div>
        </div>
        
        <div class="controls">
            <button class="btn" onclick="toggleLogging()">📝 Toggle Logging</button>
            <button class="btn" onclick="downloadLogs()">📥 Download Logs</button>
            <button class="btn" onclick="clearLogs()">🗑️ Clear Logs</button>
            <button class="btn" onclick="changeTelemetryRate()">⚡ Change Rate</button>
        </div>
    </div>
    
    <script>
        let ws;
        let chart;
        let dataPoints = [];
        const maxDataPoints = 50;
        
        function initWebSocket() {
            const wsUrl = `ws://${window.location.hostname}:81`;
            ws = new WebSocket(wsUrl);
            
            ws.onopen = function() {
                document.getElementById('connectionStatus').className = 'connection-status connected';
                document.getElementById('connectionStatus').innerHTML = '🟢 Connected';
                console.log('WebSocket connected');
            };
            
            ws.onclose = function() {
                document.getElementById('connectionStatus').className = 'connection-status disconnected';
                document.getElementById('connectionStatus').innerHTML = '🔴 Disconnected';
                console.log('WebSocket disconnected, attempting reconnect...');
                setTimeout(initWebSocket, 2000);
            };
            
            ws.onmessage = function(event) {
                try {
                    const data = JSON.parse(event.data);
                    updateDashboard(data);
                    updateChart(data);
                } catch (e) {
                    console.error('Error parsing telemetry data:', e);
                }
            };
            
            ws.onerror = function(error) {
                console.error('WebSocket error:', error);
            };
        }
        
        function updateDashboard(data) {
            // Attitude
            document.getElementById('roll').textContent = data.roll.toFixed(1) + '°';
            document.getElementById('pitch').textContent = data.pitch.toFixed(1) + '°';
            document.getElementById('yaw').textContent = data.yaw.toFixed(1) + '°';
            
            // Update attitude indicator
            const horizon = document.getElementById('horizon');
            horizon.style.transform = `rotate(${-data.roll}deg) translateY(${data.pitch}px)`;
            
            // Position
            document.getElementById('latitude').textContent = data.latitude.toFixed(6);
            document.getElementById('longitude').textContent = data.longitude.toFixed(6);
            document.getElementById('altitude').textContent = data.altitude.toFixed(1) + ' m';
            
            // Power
            document.getElementById('batteryVoltage').textContent = data.battery_voltage.toFixed(2) + ' V';
            document.getElementById('batteryCurrent').textContent = data.battery_current.toFixed(1) + ' A';
            document.getElementById('power').textContent = (data.battery_voltage * data.battery_current).toFixed(1) + ' W';
            
            // Motors
            document.getElementById('motor1').textContent = (data.motor1 * 100).toFixed(0) + '%';
            document.getElementById('motor2').textContent = (data.motor2 * 100).toFixed(0) + '%';
            document.getElementById('motor3').textContent = (data.motor3 * 100).toFixed(0) + '%';
            document.getElementById('motor4').textContent = (data.motor4 * 100).toFixed(0) + '%';
            
            // Status
            const modes = ['DISARMED', 'ACRO', 'STABILIZE', 'ALT_HOLD', 'LOITER', 'GUIDED', 'AUTO', 'RTL', 'LAND', 'FAILSAFE'];
            document.getElementById('flightMode').textContent = modes[data.flight_mode] || 'UNKNOWN';
            document.getElementById('armedStatus').textContent = data.armed ? 'ARMED' : 'DISARMED';
            document.getElementById('armedStatus').style.color = data.armed ? '#ff4444' : '#44ff44';
        }
        
        function initChart() {
            const ctx = document.getElementById('chart').getContext('2d');
            chart = new Chart(ctx, {
                type: 'line',
                data: {
                    labels: [],
                    datasets: [
                        {
                            label: 'Roll (°)',
                            data: [],
                            borderColor: '#ff6384',
                            backgroundColor: 'rgba(255, 99, 132, 0.1)',
                            tension: 0.1,
                            fill: false
                        },
                        {
                            label: 'Pitch (°)',
                            data: [],
                            borderColor: '#36a2eb',
                            backgroundColor: 'rgba(54, 162, 235, 0.1)',
                            tension: 0.1,
                            fill: false
                        },
                        {
                            label: 'Altitude (m)',
                            data: [],
                            borderColor: '#4bc0c0',
                            backgroundColor: 'rgba(75, 192, 192, 0.1)',
                            tension: 0.1,
                            fill: false,
                            yAxisID: 'y1'
                        }
                    ]
                },
                options: {
                    responsive: true,
                    maintainAspectRatio: false,
                    plugins: {
                        legend: {
                            labels: {
                                color: 'white'
                            }
                        }
                    },
                    scales: {
                        x: {
                            ticks: { color: 'white' },
                            grid: { color: 'rgba(255,255,255,0.1)' }
                        },
                        y: {
                            type: 'linear',
                            display: true,
                            position: 'left',
                            ticks: { color: 'white' },
                            grid: { color: 'rgba(255,255,255,0.1)' }
                        },
                        y1: {
                            type: 'linear',
                            display: true,
                            position: 'right',
                            ticks: { color: 'white' },
                            grid: { drawOnChartArea: false }
                        }
                    }
                }
            });
        }
        
        function updateChart(data) {
            const time = new Date().toLocaleTimeString();
            
            chart.data.labels.push(time);
            chart.data.datasets[0].data.push(data.roll);
            chart.data.datasets[1].data.push(data.pitch);
            chart.data.datasets[2].data.push(data.altitude);
            
            if (chart.data.labels.length > maxDataPoints) {
                chart.data.labels.shift();
                chart.data.datasets.forEach(dataset => dataset.data.shift());
            }
            
            chart.update('none');
        }
        
        function sendCommand(command, params = {}) {
            if (ws && ws.readyState === WebSocket.OPEN) {
                const message = { command, ...params };
                ws.send(JSON.stringify(message));
            }
        }
        
        function toggleLogging() {
            sendCommand('toggle_logging');
        }
        
        function downloadLogs() {
            window.open('/api/download_log');
        }
        
        function clearLogs() {
            if (confirm('Clear all flight logs? This cannot be undone.')) {
                sendCommand('clear_log');
            }
        }
        
        function changeTelemetryRate() {
            const rate = prompt('Enter telemetry rate (1-100 Hz):', '10');
            if (rate && !isNaN(rate)) {
                sendCommand('set_telemetry_rate', { rate: parseInt(rate) });
            }
        }
        
        // Initialize everything
        document.addEventListener('DOMContentLoaded', function() {
            initChart();
            initWebSocket();
        });
    </script>
</body>
</html>
    )HTML";
  }
};

// Использование системы телеметрии
TelemetrySystem telemetry;

void setup() {
  Serial.begin(115200);
  WiFi.begin("YourWiFi", "password");
  
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi...");
  }
  
  telemetry.begin();
}

void loop() {
  // Сбор данных полета
  TelemetrySystem::FlightData flightData;
  
  // Заполнение данных из датчиков
  flightData.roll = getCurrentRoll();
  flightData.pitch = getCurrentPitch();
  flightData.yaw = getCurrentYaw();
  flightData.altitude = getCurrentAltitude();
  flightData.latitude = getCurrentLatitude();
  flightData.longitude = getCurrentLongitude();
  flightData.batteryVoltage = getBatteryVoltage();
  flightData.batteryCurrent = getBatteryCurrent();
  flightData.flightMode = getCurrentFlightMode();
  flightData.armed = isArmed();
  
  // Обновление телеметрии
  telemetry.update(flightData);
  
  delay(20); // 50Hz основной цикл
}