---

#include <Wire.h>

#define DEV_ADDR 0x68

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

float ax, ay, az;
float wx, wy, wz;

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 roll = 0;
float pitch = 0;
float yaw = 0;

float beta_z = 0;
const float alphaFilter = 0.02;

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

unsigned long lastTime;

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

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

  return angle;
}


// Чтение данных 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();

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

  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 =====
// Переход в новый базис
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;
  }

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      aB[i] += T[i][j] * aS[j];
    }
  }

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


// ===== ЗАДАНИЕ 3 =====
// Умножение кватернионов
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;
}


void setup() {
  Serial.begin(115200);
  Wire.begin();

  Wire.beginTransmission(DEV_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Wire.beginTransmission(DEV_ADDR);
  Wire.write(0x1B);
  Wire.write(0);
  Wire.endTransmission(true);

  Wire.beginTransmission(DEV_ADDR);
  Wire.write(0x1C);
  Wire.write(0);
  Wire.endTransmission(true);

  delay(100);

  // Калибровка гироскопа
  float sum = 0;

  for (int i = 0; i < 500; i++) {
    readMPU();
    changeBasis();
    sum += r;
    delay(3);
  }

  beta_z = sum / 500.0;

  readMPU();
  changeBasis();

  roll = atan2(aBy, aBz);

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

  yaw = 0;

  // Начальный кватернион
  float cr = cos(roll / 2.0);
  float sr = sin(roll / 2.0);

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

  float cy = cos(yaw / 2.0);
  float sy = sin(yaw / 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();


  // ===== ЗАДАНИЕ 1 =====
  changeBasis();

  unsigned long now = micros();

  float dt =
    (now - lastTime) / 1000000.0;

  lastTime = now;

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

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

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

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

  Serial.println(" м/с^2");

  Serial.print("p = ");
  Serial.print(p, 4);

  Serial.print("  q = ");
  Serial.print(q, 4);

  Serial.print("  r = ");
  Serial.print(r, 4);

  Serial.println(" рад/с");


  // ===== ЗАДАНИЕ 2 =====
  // Расчёт Roll, Pitch, Yaw
  float roll_acc =
    atan2(aBy, aBz);

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

  float oldRoll = roll;
  float oldPitch = pitch;

  roll =
    (1.0 - alphaFilter) *
    (
      oldRoll +
      dt *
      (
        p
        + q * sin(oldRoll) * tan(oldPitch)
        + r * cos(oldRoll) * tan(oldPitch)
      )
    )
    + alphaFilter * roll_acc;

  pitch =
    (1.0 - alphaFilter) *
    (
      oldPitch +
      dt *
      (
        q * cos(oldRoll)
        - r * sin(oldRoll)
      )
    )
    + alphaFilter * pitch_acc;

  yaw =
    yaw + dt * (r - beta_z);

  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("===== ЗАДАНИЕ 2 =====");

  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(" град");


  // ===== ЗАДАНИЕ 3 =====
  // Обновление ориентации через кватернион
  float rCorrected = r - beta_z;

  float dThetaX = p * dt;
  float dThetaY = q * dt;
  float dThetaZ = rCorrected * dt;

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

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

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

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

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

  if (sinPitch > 1.0) {
    sinPitch = 1.0;
  }

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

  float pitchQ =
    asin(sinPitch);

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

  float rollQ_deg =
    angle360(rollQ * 180.0 / PI);

  float pitchQ_deg =
    angle360(pitchQ * 180.0 / PI);

  float yawQ_deg =
    angle360(yawQ * 180.0 / PI);

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

  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.print("Крен (Roll) = ");
  Serial.print(rollQ_deg);
  Serial.println(" град");

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

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

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

  delay(100);
}