---

#include <Wire.h>

#define DEV_ADDR 0x68

// Сырые данные MPU6050
int16_t ax_raw, ay_raw, az_raw;
int16_t gx_raw, gy_raw, gz_raw;

// Данные в базисе датчика
float ax, ay, az;
float wx, wy, wz;

// Данные после перехода в базис B
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;


// Умножение двух кватернионов
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
  );

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


// Чтение MPU6050
void readMPU() {

  Wire.beginTransmission(DEV_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);

  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();

  // Температуру здесь не используем
  Wire.read();
  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;


  // ==================================
  // Переход в базис из задания 1
  // ==================================

  aBx = -ay;
  aBy =  ax;
  aBz =  az;

  p = -wy;
  q =  wx;
  r =  wz;
}


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();

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

  float pitch0 = atan2(
    -aBx,
    sqrt(aBy*aBy + aBz*aBz)
  );

  // Начальный 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();
}


void loop() {

  readMPU();


  // ==================================
  // Время между измерениями
  // ==================================

  unsigned long now = micros();

  float dt =
    (now - lastTime) / 1000000.0;

  lastTime = now;


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

  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 s = sin(alphaStep / 2.0);

    dq0 = cos(alphaStep / 2.0);

    dq1 = (dThetaX / alphaStep) * s;
    dq2 = (dThetaY / alphaStep) * s;
    dq3 = (dThetaZ / alphaStep) * s;

  }
  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;


  // q = q / |q|
  normalizeQuaternion();


  // ==================================
  // Получаем Roll, Pitch, Yaw из q
  // ==================================

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


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

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

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

  float pitch = asin(sinPitch);


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


  // Радианы -> градусы

  float roll_deg = roll * 180.0 / PI;
  float pitch_deg = pitch * 180.0 / PI;
  float yaw_deg = yaw * 180.0 / PI;


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

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

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

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

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

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


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

  Serial.print("Roll = ");
  Serial.print(roll_deg);
  Serial.print(" deg   ");

  Serial.print("Pitch = ");
  Serial.print(pitch_deg);
  Serial.print(" deg   ");

  Serial.print("Yaw = ");
  Serial.print(yaw_deg);
  Serial.println(" deg");

  Serial.println("-----------------------------");

  delay(10);
}