// =====================================
// GPIO (GENERAL PURPOSE INPUT/OUTPUT)
// =====================================

// ЦИФРОВЫЕ ВЫХОДЫ (для LED, реле, и т.д.)
void setupDigitalOutputs() {
  pinMode(LED_PIN, OUTPUT);
  
  digitalWrite(LED_PIN, HIGH);  // Включить LED
  delay(1000);
  digitalWrite(LED_PIN, LOW);   // Выключить LED
}

// ЦИФРОВЫЕ ВХОДЫ (для кнопок, концевиков)
void setupDigitalInputs() {
  pinMode(BUTTON_PIN, INPUT_PULLUP);  // Встроенный pull-up резистор
  
  if (digitalRead(BUTTON_PIN) == LOW) {
    Serial.println("Button pressed!");
  }
}

// АНАЛОГОВЫЕ ВХОДЫ (для датчиков, потенциометров)
void setupAnalogInputs() {
  // ESP32 имеет 12-bit ADC (0-4095)
  int rawValue = analogRead(SENSOR_PIN);
  
  // Преобразование в напряжение (0-3.3V)
  float voltage = rawValue * (3.3 / 4095.0);
  
  // Преобразование в физическую величину
  float temperature = (voltage - 0.5) * 100;  // Для TMP36
  
  Serial.print("Raw: "); Serial.print(rawValue);
  Serial.print(" Voltage: "); Serial.print(voltage);
  Serial.print(" Temp: "); Serial.println(temperature);
}

// PWM ВЫХОДЫ (для моторов, сервоприводов)
void setupPWMOutputs() {
  // Настройка PWM канала
  ledcSetup(0, 50, 16);        // Канал 0, 50Hz, 16-bit
  ledcAttachPin(MOTOR_PIN, 0); // Привязать пин к каналу
  
  // Генерация PWM сигнала
  // Для ESC: 1000-2000 микросекунд (1ms-2ms)
  // При 50Hz и 16-bit: 1ms = 3277, 2ms = 6554
  
  ledcWrite(0, 3277);   // Минимальный газ
  delay(1000);
  ledcWrite(0, 4915);   // Средний газ (1.5ms)
  delay(1000);
  ledcWrite(0, 6554);   // Максимальный газ
}

// =====================================
// I2C ПРОТОКОЛ (для IMU, барометра, компаса)
// =====================================

#include <Wire.h>

// Пример работы с MPU6050 (гироскоп + акселерометр)
#define MPU6050_ADDR 0x68

void setupIMU() {
  Wire.begin();
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // Включить датчик
  Wire.endTransmission(true);
  
  Serial.println("MPU6050 initialized");
}

struct IMUData {
  float gyroX, gyroY, gyroZ;
  float accelX, accelY, accelZ;
};

IMUData readIMU() {
  IMUData data;
  
  // Запрос данных
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B);  // Начальный регистр данных
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 14, true);
  
  // Чтение сырых данных (16-bit значения)
  int16_t rawAccelX = Wire.read() << 8 | Wire.read();
  int16_t rawAccelY = Wire.read() << 8 | Wire.read();
  int16_t rawAccelZ = Wire.read() << 8 | Wire.read();
  int16_t rawTemp   = Wire.read() << 8 | Wire.read();  // Температура
  int16_t rawGyroX  = Wire.read() << 8 | Wire.read();
  int16_t rawGyroY  = Wire.read() << 8 | Wire.read();
  int16_t rawGyroZ  = Wire.read() << 8 | Wire.read();
  
  // Преобразование в физические единицы
  data.accelX = rawAccelX / 16384.0;  // ±2g range
  data.accelY = rawAccelY / 16384.0;
  data.accelZ = rawAccelZ / 16384.0;
  
  data.gyroX = rawGyroX / 131.0;      // ±250°/s range
  data.gyroY = rawGyroY / 131.0;
  data.gyroZ = rawGyroZ / 131.0;
  
  return data;
}

// =====================================
// SPI ПРОТОКОЛ (для быстрых IMU, SD карт)
// =====================================

#include <SPI.h>

#define CS_PIN 5    // Chip Select
#define SCLK_PIN 18 // Serial Clock  
#define MISO_PIN 19 // Master In Slave Out
#define MOSI_PIN 23 // Master Out Slave In

void setupSPI() {
  SPI.begin(SCLK_PIN, MISO_PIN, MOSI_PIN, CS_PIN);
  pinMode(CS_PIN, OUTPUT);
  digitalWrite(CS_PIN, HIGH);  // Неактивное состояние
  
  Serial.println("SPI initialized");
}

// Пример чтения регистра через SPI
uint8_t readSPIRegister(uint8_t address) {
  digitalWrite(CS_PIN, LOW);   // Активировать устройство
  
  SPI.transfer(address | 0x80); // Команда чтения (bit 7 = 1)
  uint8_t data = SPI.transfer(0x00); // Читаем данные
  
  digitalWrite(CS_PIN, HIGH);  // Деактивировать
  
  return data;
}

// Пример записи в регистр через SPI
void writeSPIRegister(uint8_t address, uint8_t data) {
  digitalWrite(CS_PIN, LOW);
  
  SPI.transfer(address & 0x7F); // Команда записи (bit 7 = 0)
  SPI.transfer(data);
  
  digitalWrite(CS_PIN, HIGH);
}

// =====================================
// UART ПРОТОКОЛ (для GPS, радио модулей)
// =====================================

// ESP32 имеет 3 UART порта
HardwareSerial SerialGPS(1);  // UART1 для GPS
HardwareSerial SerialRadio(2); // UART2 для радио

void setupUART() {
  // GPS обычно работает на 9600 baud
  SerialGPS.begin(9600, SERIAL_8N1, 16, 17); // RX=16, TX=17
  
  // Радио модуль на высокой скорости
  SerialRadio.begin(115200, SERIAL_8N1, 14, 15); // RX=14, TX=15
  
  Serial.println("UART initialized");
}

// Простой парсер NMEA для GPS
void parseGPS() {
  if (SerialGPS.available()) {
    String nmeaString = SerialGPS.readStringUntil('\n');
    
    if (nmeaString.startsWith("$GPGGA")) {
      // Парсинг координат из GPGGA строки
      // $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
      
      int comma1 = nmeaString.indexOf(',', 7);   // После времени
      int comma2 = nmeaString.indexOf(',', comma1 + 1);
      int comma3 = nmeaString.indexOf(',', comma2 + 1);
      int comma4 = nmeaString.indexOf(',', comma3 + 1);
      int comma5 = nmeaString.indexOf(',', comma4 + 1);
      
      String latStr = nmeaString.substring(comma1 + 1, comma2);
      String latDir = nmeaString.substring(comma2 + 1, comma3);
      String lonStr = nmeaString.substring(comma3 + 1, comma4);
      String lonDir = nmeaString.substring(comma4 + 1, comma5);
      
      if (latStr.length() > 0 && lonStr.length() > 0) {
        float latitude = parseCoordinate(latStr, latDir);
        float longitude = parseCoordinate(lonStr, lonDir);
        
        Serial.print("GPS: ");
        Serial.print(latitude, 6);
        Serial.print(", ");
        Serial.println(longitude, 6);
      }
    }
  }
}

float parseCoordinate(String coord, String direction) {
  // Конвертация DDMM.MMMM в десятичные градусы
  float degrees = coord.substring(0, 2).toFloat();
  float minutes = coord.substring(2).toFloat();
  
  float decimal = degrees + minutes / 60.0;
  
  if (direction == "S" || direction == "W") {
    decimal = -decimal;
  }
  
  return decimal;
}

// =====================================
// ПРОСТОЙ АВТОПИЛОТ НА ESP32
// =====================================

#include <Wire.h>
#include <WiFi.h>

// ===== СТРУКТУРЫ ДАННЫХ =====
struct Vector3 {
  float x, y, z;
};

struct Attitude {
  float roll, pitch, yaw;    // Углы в градусах
};

struct Position {
  float latitude, longitude, altitude;
};

struct ControlInputs {
  float throttle;  // 0.0 - 1.0
  float roll;      // -1.0 - 1.0
  float pitch;     // -1.0 - 1.0  
  float yaw;       // -1.0 - 1.0
};

struct MotorOutputs {
  float motor1, motor2, motor3, motor4;  // 0.0 - 1.0
};

// ===== ГЛОБАЛЬНЫЕ ПЕРЕМЕННЫЕ =====
Vector3 gyro, accel, mag;
Attitude attitude;
Position position;
ControlInputs controls;
MotorOutputs motors;

// Настройки PID регуляторов
float rollKp = 1.0, rollKi = 0.1, rollKd = 0.05;
float pitchKp = 1.0, pitchKi = 0.1, pitchKd = 0.05;
float yawKp = 1.0, yawKi = 0.1, yawKd = 0.05;

// Переменные для PID
float rollError, rollErrorPrev, rollErrorSum;
float pitchError, pitchErrorPrev, pitchErrorSum;
float yawError, yawErrorPrev, yawErrorSum;

unsigned long lastTime = 0;

// ===== ОСНОВНОЙ КОД =====
void setup() {
  Serial.begin(115200);
  Serial.println("=== ESP32 AUTOPILOT v1.0 ===");
  
  initHardware();
  initSensors();
  initMotors();
  
  Serial.println("Autopilot ready!");
}

void loop() {
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0;  // Время в секундах
  lastTime = currentTime;
  
  // 1. Чтение датчиков
  readSensors();
  
  // 2. Оценка состояния
  updateAttitude(dt);
  
  // 3. Чтение команд пилота
  readControlInputs();
  
  // 4. Алгоритмы управления
  calculatePIDControl(dt);
  
  // 5. Управление моторами
  updateMotors();
  
  // 6. Отправка телеметрии
  sendTelemetry();
  
  delay(20);  // 50Hz основной цикл
}

// =====================================
// PID РЕГУЛЯТОР ДЛЯ СТАБИЛИЗАЦИИ
// =====================================

void calculatePIDControl(float dt) {
  // Желаемые углы на основе команд пилота
  float desiredRoll = controls.roll * 30.0;   // Максимум ±30°
  float desiredPitch = controls.pitch * 30.0;
  float desiredYawRate = controls.yaw * 180.0; // Максимум ±180°/с
  
  // === ROLL PID ===
  rollError = desiredRoll - attitude.roll;
  rollErrorSum += rollError * dt;
  rollErrorSum = constrain(rollErrorSum, -10, 10);  // Anti-windup
  
  float rollDerivative = (rollError - rollErrorPrev) / dt;
  rollErrorPrev = rollError;
  
  float rollOutput = rollKp * rollError + 
                     rollKi * rollErrorSum + 
                     rollKd * rollDerivative;
  
  // === PITCH PID ===
  pitchError = desiredPitch - attitude.pitch;
  pitchErrorSum += pitchError * dt;
  pitchErrorSum = constrain(pitchErrorSum, -10, 10);
  
  float pitchDerivative = (pitchError - pitchErrorPrev) / dt;
  pitchErrorPrev = pitchError;
  
  float pitchOutput = pitchKp * pitchError + 
                      pitchKi * pitchErrorSum + 
                      pitchKd * pitchDerivative;
  
  // === YAW PID (на угловой скорости) ===
  yawError = desiredYawRate - gyro.z;
  yawErrorSum += yawError * dt;
  yawErrorSum = constrain(yawErrorSum, -10, 10);
  
  float yawDerivative = (yawError - yawErrorPrev) / dt;
  yawErrorPrev = yawError;
  
  float yawOutput = yawKp * yawError + 
                    yawKi * yawErrorSum + 
                    yawKd * yawDerivative;
  
  // === МИКШИРОВАНИЕ НА МОТОРЫ ===
  // Квадрокоптер X конфигурация:
  // Motor1 (передний правый): +pitch, -roll, -yaw
  // Motor2 (задний правый):   -pitch, -roll, +yaw  
  // Motor3 (задний левый):    -pitch, +roll, -yaw
  // Motor4 (передний левый):  +pitch, +roll, +yaw
  
  float throttle = controls.throttle;
  
  motors.motor1 = throttle + pitchOutput - rollOutput - yawOutput;
  motors.motor2 = throttle - pitchOutput - rollOutput + yawOutput;
  motors.motor3 = throttle - pitchOutput + rollOutput - yawOutput;
  motors.motor4 = throttle + pitchOutput + rollOutput + yawOutput;
  
  // Ограничение выходов 0.0-1.0
  motors.motor1 = constrain(motors.motor1, 0.0, 1.0);
  motors.motor2 = constrain(motors.motor2, 0.0, 1.0);
  motors.motor3 = constrain(motors.motor3, 0.0, 1.0);
  motors.motor4 = constrain(motors.motor4, 0.0, 1.0);
}

// Функция ограничения значений
float constrain(float value, float min_val, float max_val) {
  if (value < min_val) return min_val;
  if (value > max_val) return max_val;
  return value;
}

// =====================================
// КОМПЛЕМЕНТАРНЫЙ ФИЛЬТР
// =====================================

void updateAttitude(float dt) {
  // Интегрирование гироскопа (быстро, но дрейфует)
  attitude.roll += gyro.x * dt;
  attitude.pitch += gyro.y * dt;
  attitude.yaw += gyro.z * dt;
  
  // Расчет углов из акселерометра (медленно, но точно)
  float accelRoll = atan2(accel.y, accel.z) * 180.0 / PI;
  float accelPitch = atan2(-accel.x, sqrt(accel.y*accel.y + accel.z*accel.z)) * 180.0 / PI;
  
  // Комплементарный фильтр (объединяем показания)
  float alpha = 0.98;  // Коэффициент доверия гироскопу
  
  attitude.roll = alpha * attitude.roll + (1.0 - alpha) * accelRoll;
  attitude.pitch = alpha * attitude.pitch + (1.0 - alpha) * accelPitch;
  
  // Yaw можно корректировать по магнитометру
  if (mag.x != 0 || mag.y != 0) {
    float magYaw = atan2(mag.y, mag.x) * 180.0 / PI;
    attitude.yaw = alpha * attitude.yaw + (1.0 - alpha) * magYaw;
  }
  
  // Ограничение углов в диапазоне ±180°
  if (attitude.roll > 180) attitude.roll -= 360;
  if (attitude.roll < -180) attitude.roll += 360;
  if (attitude.pitch > 180) attitude.pitch -= 360;
  if (attitude.pitch < -180) attitude.pitch += 360;
  if (attitude.yaw > 180) attitude.yaw -= 360;
  if (attitude.yaw < -180) attitude.yaw += 360;
}

// =====================================
// РЕЖИМЫ ПОЛЕТА
// =====================================

enum FlightMode {
  MANUAL,      // Ручное управление
  STABILIZE,   // Стабилизация углов
  ALT_HOLD,    // Удержание высоты
  LOITER,      // Зависание в точке
  AUTO,        // Автоматический полет
  RTL          // Возврат домой
};

FlightMode currentMode = STABILIZE;
Position homePosition;
bool isArmed = false;

void processFlightMode() {
  switch (currentMode) {
    case MANUAL:
      // Прямое управление моторами
      manualMode();
      break;
      
    case STABILIZE:
      // Стабилизация углов (уже реализована выше)
      stabilizeMode();
      break;
      
    case ALT_HOLD:
      // Стабилизация + удержание высоты
      altHoldMode();
      break;
      
    case LOITER:
      // Стабилизация + удержание позиции
      loiterMode();
      break;
      
    case AUTO:
      // Автоматический полет по waypoints
      autoMode();
      break;
      
    case RTL:
      // Возврат домой
      returnToLaunchMode();
      break;
  }
}

void manualMode() {
  // Прямое преобразование команд в PWM
  motors.motor1 = controls.throttle;
  motors.motor2 = controls.throttle;
  motors.motor3 = controls.throttle;
  motors.motor4 = controls.throttle;
  
  // Добавляем управление по осям
  motors.motor1 += controls.pitch - controls.roll - controls.yaw;
  motors.motor2 += -controls.pitch - controls.roll + controls.yaw;
  motors.motor3 += -controls.pitch + controls.roll - controls.yaw;
  motors.motor4 += controls.pitch + controls.roll + controls.yaw;
  
  // Ограничиваем
  motors.motor1 = constrain(motors.motor1, 0.0, 1.0);
  motors.motor2 = constrain(motors.motor2, 0.0, 1.0);
  motors.motor3 = constrain(motors.motor3, 0.0, 1.0);
  motors.motor4 = constrain(motors.motor4, 0.0, 1.0);
}

void stabilizeMode() {
  // Уже реализована в calculatePIDControl()
  calculatePIDControl(0.02);  // 50Hz
}

void altHoldMode() {
  // Сначала стабилизация углов
  stabilizeMode();
  
  // Затем контроль высоты
  static float targetAltitude = position.altitude;
  static bool altitudeSet = false;
  
  // Установка целевой высоты при первом входе в режим
  if (!altitudeSet) {
    targetAltitude = position.altitude;
    altitudeSet = true;
  }
  
  // Изменение целевой высоты стиком газа
  if (abs(controls.throttle - 0.5) > 0.1) {
    targetAltitude += (controls.throttle - 0.5) * 2.0;  // 2 м/с максимум
    altitudeSet = false;
  }
  
  // PID регулятор высоты
  static float altError, altErrorPrev, altErrorSum;
  float altKp = 0.5, altKi = 0.1, altKd = 0.2;
  
  altError = targetAltitude - position.altitude;
  altErrorSum += altError * 0.02;
  altErrorSum = constrain(altErrorSum, -5, 5);
  
  float altDerivative = (altError - altErrorPrev) / 0.02;
  altErrorPrev = altError;
  
  float altOutput = altKp * altError + altKi * altErrorSum + altKd * altDerivative;
  
  // Модификация throttle для всех моторов
  motors.motor1 += altOutput;
  motors.motor2 += altOutput;
  motors.motor3 += altOutput;
  motors.motor4 += altOutput;
  
  // Ограничение
  motors.motor1 = constrain(motors.motor1, 0.0, 1.0);
  motors.motor2 = constrain(motors.motor2, 0.0, 1.0);
  motors.motor3 = constrain(motors.motor3, 0.0, 1.0);
  motors.motor4 = constrain(motors.motor4, 0.0, 1.0);
}

void returnToLaunchMode() {
  // Простая реализация RTL
  
  // 1. Подняться на безопасную высоту
  float safeAltitude = homePosition.altitude + 20.0;  // +20 метров
  if (position.altitude < safeAltitude) {
    // Подъем
    controls.throttle = 0.7;
    stabilizeMode();
    return;
  }
  
  // 2. Лететь к точке старта
  float distanceToHome = sqrt(
    pow(position.latitude - homePosition.latitude, 2) +
    pow(position.longitude - homePosition.longitude, 2)
  );
  
  if (distanceToHome > 0.00001) {  // ~1 метр точности GPS
    // Вычисляем направление к дому
    float bearingToHome = atan2(
      homePosition.longitude - position.longitude,
      homePosition.latitude - position.latitude
    );
    
    // Устанавливаем команды для полета домой
    controls.pitch = cos(bearingToHome) * 0.3;  // 30% максимального наклона
    controls.roll = sin(bearingToHome) * 0.3;
    controls.yaw = 0;
    controls.throttle = 0.5;  // Постоянная высота
    
    stabilizeMode();
  } else {
    // 3. Посадка над домом
    controls.throttle = 0.3;  // Медленное снижение
    controls.pitch = 0;
    controls.roll = 0;
    controls.yaw = 0;
    
    stabilizeMode();
    
    // Disarm при касании земли
    if (position.altitude <= homePosition.altitude + 0.5) {
      isArmed = false;
    }
  }
}

// ЗАДАЧА: Создать систему сбора и передачи данных датчиков

/* ТРЕБОВАНИЯ:
1. Подключить IMU датчик по I2C
2. Подключить GPS по UART
3. Настроить WiFi для передачи данных
4. Создать web-интерфейс для просмотра данных в реальном времени
5. Логирование данных на SD карту

КРИТЕРИИ ОЦЕНКИ:
- Корректность подключения датчиков (25%)
- Качество программного кода (30%)
- Функциональность web-интерфейса (25%)
- Стабильность работы системы (20%)
*/

#include <WiFi.h>
#include <WebServer.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>

// Ваш код здесь...

// ЗАДАЧА: Реализовать базовый автопилот для стабилизации

/* ТРЕБОВАНИЯ:
1. Чтение данных IMU с частотой 100Hz
2. Реализация PID регулятора для стабилизации roll/pitch
3. Генерация PWM сигналов для 4 моторов
4. Режимы: Manual, Stabilize, Disarmed
5. Безопасность: автоматическое отключение при потере сигнала

ТЕСТИРОВАНИЕ:
- Симуляция на стенде с сервоприводами
- Проверка отклика на внешние возмущения
- Анализ качества стабилизации

КРИТЕРИИ ОЦЕНКИ:
- Точность чтения датчиков (20%)
- Корректность PID алгоритма (35%)
- Качество генерации PWM (20%)
- Реализация систем безопасности (25%)
*/

// Ваша реализация автопилота...

// Калибровка магнитометра
// Автоматическое определение типа GPS
// Фильтр Калмана для sensor fusion
// Обнаружение отказов датчиков

// Использование DMA для высокоскоростного чтения
// Многозадачность с FreeRTOS
// Оптимизация вычислений с плавающей точкой
// Профилирование производительности

// MAVLink протокол телеметрии
// Mesh-сети между дронами  
// Прямая связь с GCS
// OTA обновления прошивки

// Watchdog таймеры
// Резервирование критических систем
// Детекция аномального поведения
// Emergency recovery процедуры