#include <MPU6050_6Axis_MotionApps20.h>
#include <Wire.h>
#include <Servo.h>
#include <GyverPID.h>

// Пины сервоприводов
#define AILERON_LEFT_PIN 9
#define AILERON_RIGHT_PIN 10
#define ELEVATOR_PIN 11
#define RUDDER_PIN 12

// Объекты сервоприводов
Servo aileron_left, aileron_right, elevator, rudder;

// ПИД-регуляторы (Kp, Ki, Kd)
GyverPID pid_roll(6.0, 0.2, 20.0);   // Крен
GyverPID pid_pitch(5.0, 0.1, 15.0); // Тангаж
GyverPID pid_yaw(3.0, 0.05, 8.0);    // Рыскание

// MPU6050 и DMP
MPU6050 mpu;
bool dmpReady = false;
uint16_t packetSize;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3]; // [yaw, pitch, roll]

// Тайминг
uint32_t prevUpdate = 0;
const uint16_t UPDATE_INTERVAL = 10; // 10 мс (100 Гц)

// Эмулированные параметры
float target_altitude = 100.0;

// ================================================================
// Настройка
// ================================================================
void setup() {
  Wire.begin();
  Wire.setClock(400000);
  Serial.begin(115200);

  // Инициализация MPU6050
  mpu.initialize();
  uint8_t devStatus = mpu.dmpInitialize();
  
  // Калибровка (подставьте свои значения!)
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);

  if (devStatus == 0) {
    mpu.setDMPEnabled(true);
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    while(1); // Остановка при ошибке инициализации
  }

  // Инициализация сервоприводов
  aileron_left.attach(AILERON_LEFT_PIN);
  aileron_right.attach(AILERON_RIGHT_PIN);
  elevator.attach(ELEVATOR_PIN);
  rudder.attach(RUDDER_PIN);

  // Настройка PID
  pid_roll.setDirection(NORMAL);
  pid_pitch.setDirection(NORMAL);
  pid_yaw.setDirection(NORMAL);
  
  pid_roll.setLimits(-50, 50);
  pid_pitch.setLimits(-30, 30);
  pid_yaw.setLimits(-25, 25);
}

// ================================================================
// Основной цикл
// ================================================================
void loop() {
  // Таймер на 10 мс
  if (millis() - prevUpdate >= UPDATE_INTERVAL) {
    prevUpdate = millis();
    
    // Чтение данных с DMP
    if (dmpReady && mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      
      // Конвертация в градусы
      float roll = ypr[2] * 180/M_PI;
      float pitch = ypr[1] * 180/M_PI;
      float yaw = ypr[0] * 180/M_PI;

      // Управление высотой через тангаж
      pid_pitch.setpoint = calculate_pitch_setpoint(target_altitude);
      
      // Обновление PID с авторасчетом dt
      pid_roll.input = roll;
      pid_pitch.input = pitch;
      pid_yaw.input = yaw;
      
      pid_roll.getResultNow(); // Автоматически считает dt
      pid_pitch.getResultNow();
      pid_yaw.getResultNow();

      // Управление сервами
      control_ailerons(pid_roll.output);
      control_elevator(pid_pitch.output);
      control_rudder(pid_yaw.output);
    }
  }
}

// ================================================================
// Управление элеронами (противофазное)
// ================================================================
void control_ailerons(float pid_output) {
  int left_angle = 90 + pid_output;   // Нейтрал 90°
  int right_angle = 90 - pid_output;
  
  aileron_left.write(constrain(left_angle, 0, 180));
  aileron_right.write(constrain(right_angle, 0, 180));
}

// ================================================================
// Управление рулём высоты
// ================================================================
void control_elevator(float pid_output) {
  int angle = 90 + pid_output;
  elevator.write(constrain(angle, 0, 180));
}

// ================================================================
// Управление рулём направления
// ================================================================
void control_rudder(float pid_output) {
  int angle = 90 + pid_output;
  rudder.write(constrain(angle, 0, 180));
}

// ================================================================
// Эмуляция высоты и расчёт тангажа
// ================================================================
float calculate_pitch_setpoint(float target_alt) {
  static float current_alt = 100.0;  // Эмуляция текущей высоты
  float alt_error = target_alt - current_alt;
  return constrain(alt_error * 0.5, -15, 15);
}