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

template<int samplesNum>
class AverageFilter
{
  public:
  AverageFilter(MPU6050 *obj, int16_t (MPU6050::*getFunction)())
  {
    _mpu = obj;
    _getFunction = getFunction;
  }
  ~AverageFilter() = default;

  double operator()()
  {
    sum -= samples[curIndex];
    samples[curIndex] = (_mpu->*_getFunction)();
    sum += samples[curIndex];
    curIndex = (curIndex + 1) % samplesNum;  
    return (double)sum/samplesNum;
  }

  private:
  MPU6050 *_mpu;
  int (MPU6050::*_getFunction)();
  long sum = 0;
  int samples[samplesNum]{};
  int curIndex = 0;
};

LiquidCrystal lcd(13, 12, 11, 10, 9, 8);
MPU6050 mpu;

AverageFilter<10> filterX(&mpu, &MPU6050::getRotationX);
AverageFilter<10> filterY(&mpu, &MPU6050::getRotationY);
AverageFilter<10> filterZ(&mpu, &MPU6050::getRotationZ);

void process()
{
int rotX = round(filterX()/32768*250);
int rotY = round(filterY()/32768*250);
int rotZ = round(filterZ()/32768*250);

//преобразование в char и вывод на дисплей
const int kSize = 5;
char bufer[kSize];

lcd.setCursor(0,1);
dtostrf(rotX, -1, 0, bufer);
print(bufer, kSize);
lcd.setCursor(4,1);
lcd.print("|");
dtostrf(rotY, -1, 0, bufer);
print(bufer, kSize);
lcd.setCursor(9,1);
lcd.print("|");
dtostrf(rotZ, -1, 0, bufer);
print(bufer, kSize);
}

//вспомогательная функция печати на дисплей, чтобы не выполнять clear каждый новый цикл (защита от мерцания)
void print(char *bufer, int size)
{
  bool endString = false;
  for(int i = 0; i < size; ++i)
  {
    if (bufer[i] == '\0')
    {
      endString = true;
    }
    if(endString)
    {
      lcd.print(" ");
    }    
    else
    {
      lcd.print(bufer[i]);
    }    
  }
}
void setup() 
{
  analogReference(DEFAULT);
  lcd.begin(16, 2, LCD_5x8DOTS);
  lcd.clear();
  lcd.print("X   |Y   |Z  ");
  mpu.initialize();
  Serial.begin(9600);
  //проверка соединения
  //Serial.println(mpu.testConnection() ? "MPU6050 OK" : "MPU6050 FAIL");
}

void loop() 
{
  process();  
  delay(100);
}