#include <I2Cdev.h>
#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, dt)
GyverPID pid_roll(6.0, 0.2, 20.0, 10);   // Крен
GyverPID pid_pitch(5.0, 0.1, 15.0, 10); // Тангаж
GyverPID pid_yaw(3.0, 0.05, 8.0, 10);   // Рыскание

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

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

// ================================================================
// Прерывание для DMP
// ================================================================
void dmpDataReady() {
  mpuInterrupt = true;
}

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

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

  if (devStatus == 0) {
    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.dmpGetFIFOPacketSize();
  }

  // Инициализация сервоприводов
  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() {
  if (!dmpReady) return;

  // Получение данных с DMP
  if (mpuInterrupt && fifoCount < packetSize) {
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      mpu.resetFIFO();
    } else if (mpuIntStatus & 0x02) {
      while (fifoCount >= packetSize) {
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
      }

      // Получение углов Эйлера
      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
      pid_roll.input = roll;
      pid_pitch.input = pitch;
      pid_yaw.input = yaw;

      pid_roll.getResult();
      pid_pitch.getResult();
      pid_yaw.getResult();

      // Управление сервами
      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);
}