//Комплементарный фильтр
//https://robotclass.ru/articles/complementary-filter/
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
#include <Wire.h>

#define TO_DEG 57.29577951308232087679815481410517033f
#define T_OUT 20 // каждый 20 миллисекунд будем проводить вычисления 
#define P_OUT 50 // каждый 50 миллисекунд будем выводить данные
#define FK 0.1 // коэффициент комплементарного фильтра

#define VERT_PIN A1
#define HORZ_PIN A0
#define ZERO_VERT 512
#define ZERO_HORZ 512

MPU6050 accelgyro;
Servo servo1;
Servo servo2;
Servo servo3;

float angle_ax, angle_gx, angle_cpl, angle_ay, angle_gy, angle_az, angle_gz;
float x, y, z;
int dt = 0;
long int t_next, p_next;
int horz;
int vert;

// функция, которая не даёт значению выйти за пределы
float clamp(float v, float minv, float maxv) {
  if ( v > maxv )
    return maxv;
  else if ( v < minv )
    return minv;
  return v;
}

void setup() {
  Wire.begin();
  Serial.begin(9600);
  servo1.attach(11);
  servo2.attach(10);
  servo3.attach(9);
  // инициализация MPU6050
  accelgyro.initialize();
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  pinMode(VERT_PIN, INPUT);
  pinMode(HORZ_PIN, INPUT);

  delay(1000);
}

void loop() {

  horz = analogRead(HORZ_PIN);
  vert = analogRead(VERT_PIN);
  Serial.print("HORZ ");
  Serial.print(horz);
  Serial.print("  VERT ");
  Serial.println(vert);
  //delay(500);
  if (ZERO_VERT != vert or ZERO_HORZ != horz) {
    horz = map(horz, 0, 1023, 0, 180);
    vert = map(vert, 0, 1023, 0, 180);
    //Serial.print("HORZ ");
    //Serial.print(horz);
    //Serial.print("  VERT ");
    //Serial.println(vert);
    servo1.write(horz);
    servo2.write(vert);
  }
  else {
    long int t = millis();
    // каждые T_TO миллисекунд выполняем рассчет угла наклона
    if ( t_next < t ) {
      int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
      float ay, gx, ax, gy, az, gz;

      t_next = t + T_OUT;
      // получаем сырые данные от датчиков в MPU6050
      accelgyro.getMotion6(&ax_raw, &ay_raw, &az_raw, &gx_raw, &gy_raw, &gz_raw);

      // преобразуем сырые данные гироскопа в град/сек
      gx = gx_raw / 16.4;
      gy = gy_raw / 16.4;
      gz = gz_raw / 16.4;
      // преобразуем сырые данные акселерометра в G
      ay = ay_raw / 4096.0;
      ay = clamp(ay, -1.0, 1.0);
      ax = ax_raw / 4096.0;
      ax = clamp(ax, -1.0, 1.0);
      az = az_raw / 4096.0;
      az = clamp(az, -1.0, 1.0);

      // вычисляем угол наклона по акселерометру
      angle_ax = 90 - TO_DEG * acos(ay);
      angle_ay = 90 - TO_DEG * acos(ax);
      angle_az = 90 - TO_DEG * acos(az);
      // вычисляем угол наклона по гироскопу
      angle_gx = angle_gx + gx * T_OUT / 1000.0;
      angle_gy = angle_gy + gy * T_OUT / 1000.0;
      angle_gz = angle_gz + gz * T_OUT / 1000.0;
      // корректируем значение угла с помощью акселерометра
      angle_gx = angle_gx * (1 - FK) + angle_ax * FK;
      angle_gy = angle_gy * (1 - FK) + angle_ay * FK;
      angle_gz = angle_gz * (1 - FK) + angle_az * FK;
    }

    t = millis();
    // каждые P_OUT миллисекунд выводим результат в COM порт
    if ( p_next < t ) {
      p_next = t + P_OUT;
      //Serial.print("X ");
      //Serial.print(angle_gx);
      x = map(angle_gx, -90, 90, 0, 180);
      servo1.write(x);

      //Serial.print("  Y ");
      //Serial.print(angle_gy);
      y = map(angle_gy, -90, 90, 0, 180);
      //Serial.print("  Y ");
      //Serial.println(y);
      //horz > y ? y += horz : y -= horz;
      servo2.write(y);

      //Serial.print("  Z ");
      //Serial.println(angle_gz);
      z = angle_gz;
      servo3.write( map(z, -90, 90, 0, 180) );
      servo3.write(z);
    }

  }

  delay(50); //Нужно закоментировать. Это фишка для отладки в эмуляторе WOKWI
}