---

#include <Wire.h>

#define DEV_ADDR 0x68

// ==========================================
// СЫРЫЕ ДАННЫЕ MPU6050
// ==========================================

int16_t ax_raw, ay_raw, az_raw;
int16_t temp_raw;
int16_t gx_raw, gy_raw, gz_raw;


// ==========================================
// ИСХОДНЫЕ ФИЗИЧЕСКИЕ ДАННЫЕ
// ==========================================

// Ускорение, м/с^2
float ax, ay, az;

// Угловая скорость, рад/с
float wx, wy, wz;


// ==========================================
// МАТРИЦА ПЕРЕХОДА
//
//       | 0  -1  0 |
// T  =  | 1   0  0 |
//       | 0   0  1 |
//
// ==========================================

float T[3][3] = {
  {0, -1, 0},
  {1,  0, 0},
  {0,  0, 1}
};


// Векторы
float aS[3];
float wS[3];

float aB[3];
float wB[3];


// После перехода в новый базис
float aBx, aBy, aBz;

// Угловые скорости нового базиса
float p, q, r;


// ==========================================
// КВАТЕРНИОН ОРИЕНТАЦИИ
// ==========================================

float q0 = 1.0;
float q1 = 0.0;
float q2 = 0.0;
float q3 = 0.0;


// Время
unsigned long lastTime;


// ==========================================
// УГОЛ 0...360
// ==========================================

float angle360(float angle) {

  while (angle < 0) {
    angle += 360.0;
  }

  while (angle >= 360.0) {
    angle -= 360.0;
  }

  return angle;
}


// ==========================================
// УМНОЖЕНИЕ КВАТЕРНИОНОВ
//
// c = a ⊗ b
// ==========================================

void quatMultiply(
  float a0, float a1, float a2, float a3,
  float b0, float b1, float b2, float b3,
  float &c0, float &c1, float &c2, float &c3
) {

  c0 = a0*b0 - a1*b1 - a2*b2 - a3*b3;

  c1 = a0*b1 + a1*b0 + a2*b3 - a3*b2;

  c2 = a0*b2 - a1*b3 + a2*b0 + a3*b1;

  c3 = a0*b3 + a1*b2 - a2*b1 + a3*b0;
}


// ==========================================
// НОРМИРОВКА КВАТЕРНИОНА
// ==========================================

void normalizeQuaternion() {

  float norm = sqrt(
    q0*q0 +
    q1*q1 +
    q2*q2 +
    q3*q3
  );

  if (norm == 0) return;

  q0 /= norm;
  q1 /= norm;
  q2 /= norm;
  q3 /= norm;
}


// ==========================================
// ЧТЕНИЕ MPU6050
// ==========================================

void readMPU() {

  Wire.beginTransmission(DEV_ADDR);

  // Начинаем с ACCEL_XOUT_H
  Wire.write(0x3B);

  Wire.endTransmission(false);

  // Читаем 14 байт
  Wire.requestFrom(DEV_ADDR, 14, true);


  // Акселерометр
  ax_raw = (Wire.read() << 8) | Wire.read();
  ay_raw = (Wire.read() << 8) | Wire.read();
  az_raw = (Wire.read() << 8) | Wire.read();


  // Температура
  temp_raw = (Wire.read() << 8) | Wire.read();


  // Гироскоп
  gx_raw = (Wire.read() << 8) | Wire.read();
  gy_raw = (Wire.read() << 8) | Wire.read();
  gz_raw = (Wire.read() << 8) | Wire.read();


  // ========================================
  // ФИЗИЧЕСКИЕ ЕДИНИЦЫ
  // ========================================

  // Ускорение -> м/с^2
  ax = ax_raw / 16384.0 * 9.81;
  ay = ay_raw / 16384.0 * 9.81;
  az = az_raw / 16384.0 * 9.81;


  // Гироскоп -> рад/с
  wx = gx_raw / 131.0 * PI / 180.0;
  wy = gy_raw / 131.0 * PI / 180.0;
  wz = gz_raw / 131.0 * PI / 180.0;
}


// ==========================================
// ПЕРЕХОД В НОВЫЙ БАЗИС
// МАТРИЧНОЕ УМНОЖЕНИЕ
// ==========================================

void changeBasis() {

  aS[0] = ax;
  aS[1] = ay;
  aS[2] = az;

  wS[0] = wx;
  wS[1] = wy;
  wS[2] = wz;


  // Обнуляем результат
  for (int i = 0; i < 3; i++) {
    aB[i] = 0;
    wB[i] = 0;
  }


  // aB = T * aS
  for (int i = 0; i < 3; i++) {

    for (int j = 0; j < 3; j++) {

      aB[i] += T[i][j] * aS[j];
    }
  }


  // wB = T * wS
  for (int i = 0; i < 3; i++) {

    for (int j = 0; j < 3; j++) {

      wB[i] += T[i][j] * wS[j];
    }
  }


  aBx = aB[0];
  aBy = aB[1];
  aBz = aB[2];


  p = wB[0];
  q = wB[1];
  r = wB[2];
}


// ==========================================
// SETUP
// ==========================================

void setup() {

  Serial.begin(115200);

  Wire.begin();


  // Разбудить MPU6050
  Wire.beginTransmission(DEV_ADDR);

  Wire.write(0x6B);

  Wire.write(0);

  Wire.endTransmission(true);


  // Гироскоп ±250 град/с
  Wire.beginTransmission(DEV_ADDR);

  Wire.write(0x1B);

  Wire.write(0);

  Wire.endTransmission(true);


  // Акселерометр ±2g
  Wire.beginTransmission(DEV_ADDR);

  Wire.write(0x1C);

  Wire.write(0);

  Wire.endTransmission(true);


  delay(100);


  // ========================================
  // НАЧАЛЬНАЯ ОРИЕНТАЦИЯ
  // ========================================

  readMPU();

  changeBasis();


  // Начальный Roll по акселерометру
  float roll0 =
    atan2(aBy, aBz);


  // Начальный Pitch по акселерометру
  float pitch0 =
    atan2(
      -aBx,
      sqrt(aBy*aBy + aBz*aBz)
    );


  // Yaw из акселерометра получить нельзя,
  // поэтому принимаем начальный Yaw = 0
  float yaw0 = 0;


  // ========================================
  // ROLL, PITCH, YAW -> КВАТЕРНИОН
  // ========================================

  float cr = cos(roll0 / 2.0);
  float sr = sin(roll0 / 2.0);

  float cp = cos(pitch0 / 2.0);
  float sp = sin(pitch0 / 2.0);

  float cy = cos(yaw0 / 2.0);
  float sy = sin(yaw0 / 2.0);


  q0 = cr*cp*cy + sr*sp*sy;

  q1 = sr*cp*cy - cr*sp*sy;

  q2 = cr*sp*cy + sr*cp*sy;

  q3 = cr*cp*sy - sr*sp*cy;


  normalizeQuaternion();


  lastTime = micros();
}


// ==========================================
// LOOP
// ==========================================

void loop() {

  readMPU();

  changeBasis();


  // ========================================
  // ВРЕМЯ dt
  // ========================================

  unsigned long now = micros();

  float dt =
    (now - lastTime) / 1000000.0;

  lastTime = now;


  // ========================================
  // ЗАДАНИЕ 3
  //
  // Δθ = ω * dt
  // ========================================

  float dThetaX = p * dt;

  float dThetaY = q * dt;

  float dThetaZ = r * dt;


  // ========================================
  // МОДУЛЬ МАЛОГО ПОВОРОТА
  //
  // alpha = |Δθ|
  // ========================================

  float alphaStep = sqrt(
    dThetaX*dThetaX +
    dThetaY*dThetaY +
    dThetaZ*dThetaZ
  );


  // ========================================
  // КВАТЕРНИОН МАЛОГО ПОВОРОТА Δq
  // ========================================

  float dq0;
  float dq1;
  float dq2;
  float dq3;


  if (alphaStep > 0.000001) {

    float sinHalf =
      sin(alphaStep / 2.0);


    dq0 =
      cos(alphaStep / 2.0);


    dq1 =
      (dThetaX / alphaStep) * sinHalf;


    dq2 =
      (dThetaY / alphaStep) * sinHalf;


    dq3 =
      (dThetaZ / alphaStep) * sinHalf;
  }

  else {

    // Если поворот практически отсутствует
    dq0 = 1.0;

    dq1 = 0.0;

    dq2 = 0.0;

    dq3 = 0.0;
  }


  // ========================================
  // q(k+1) = q(k) ⊗ Δq
  // ========================================

  float newQ0;
  float newQ1;
  float newQ2;
  float newQ3;


  quatMultiply(
    q0, q1, q2, q3,

    dq0, dq1, dq2, dq3,

    newQ0, newQ1, newQ2, newQ3
  );


  // Сохраняем новый кватернион

  q0 = newQ0;

  q1 = newQ1;

  q2 = newQ2;

  q3 = newQ3;


  // ========================================
  // НОРМИРОВКА
  // ========================================

  normalizeQuaternion();


  // ========================================
  // КВАТЕРНИОН -> ROLL
  // ========================================

  float roll = atan2(
    2.0 * (q0*q1 + q2*q3),

    1.0 - 2.0 * (q1*q1 + q2*q2)
  );


  // ========================================
  // КВАТЕРНИОН -> PITCH
  // ========================================

  float sinPitch =
    2.0 * (q0*q2 - q3*q1);


  // Защита от ошибок округления
  if (sinPitch > 1.0) {
    sinPitch = 1.0;
  }

  if (sinPitch < -1.0) {
    sinPitch = -1.0;
  }


  float pitch =
    asin(sinPitch);


  // ========================================
  // КВАТЕРНИОН -> YAW
  // ========================================

  float yaw = atan2(
    2.0 * (q0*q3 + q1*q2),

    1.0 - 2.0 * (q2*q2 + q3*q3)
  );


  // ========================================
  // РАДИАНЫ -> ГРАДУСЫ
  // 0...360
  // ========================================

  float roll_deg =
    angle360(roll * 180.0 / PI);


  float pitch_deg =
    angle360(pitch * 180.0 / PI);


  float yaw_deg =
    angle360(yaw * 180.0 / PI);


  // ========================================
  // ВЫВОД
  // ========================================

  Serial.println("===== ЗАДАНИЕ 3 =====");


  Serial.println("Кватернион:");

  Serial.print("q0 = ");
  Serial.print(q0, 5);

  Serial.print("  q1 = ");
  Serial.print(q1, 5);

  Serial.print("  q2 = ");
  Serial.print(q2, 5);

  Serial.print("  q3 = ");
  Serial.println(q3, 5);


  Serial.println("Углы:");

  Serial.print("Крен (Roll) = ");
  Serial.print(roll_deg);
  Serial.println(" град");


  Serial.print("Тангаж (Pitch) = ");
  Serial.print(pitch_deg);
  Serial.println(" град");


  Serial.print("Рыскание (Yaw) = ");
  Serial.print(yaw_deg);
  Serial.println(" град");


  Serial.println("==============================");


  delay(20);
}