#include <LiquidCrystal.h>
#include "MPU6050.h"

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);
MPU6050 mpu;
int16_t ax, ay, az;

void setup() {
  mpu.initialize();
  lcd.begin(16, 2);
  lcd.clear();
  lcd.print("X-ang:");
  lcd.setCursor(0, 1);
  lcd.print("Y-ang:");
}

float Filter(int16_t x, int j)
{
  static float arr[3][10] = {0};
  float temp = 0;
  arr[j][0] = x;
  for(const auto& el : arr[j])
  {
    temp += el;
  }

  for(int i = 9; i > 0; i--)
  {
    arr[j][i] = arr[j][i-1];
  }
  return (temp/10);
}

void loop() {
  mpu.getAcceleration(&ax, &ay, &az);
  lcd.setCursor(7, 0);
  lcd.print("      ");
  lcd.setCursor(7, 1);
  lcd.print("      ");
  float angleX = atan2(Filter(ay, 1), Filter(az, 2)) * 180 / PI;
  float angleY = atan2(Filter(ax, 0), Filter(az, 2)) * 180 / PI;
  lcd.setCursor(7, 0);
  lcd.print(angleX);
  lcd.setCursor(7, 1);
  lcd.print(angleY);
  delay(100);
}