#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 mpu1;
MPU6050 mpu2(0x69);

AverageFilter<10> filterX1(&mpu1, &MPU6050::getRotationX);
AverageFilter<10> filterY1(&mpu1, &MPU6050::getRotationY);
AverageFilter<10> filterZ1(&mpu1, &MPU6050::getRotationZ);

AverageFilter<10> filterX2(&mpu2, &MPU6050::getRotationX);
AverageFilter<10> filterY2(&mpu2, &MPU6050::getRotationY);
AverageFilter<10> filterZ2(&mpu2, &MPU6050::getRotationZ);

void process()
{
int rotX1 = round(filterX1()/32768*250);
int rotY1 = round(filterY1()/32768*250);
int rotZ1 = round(filterZ1()/32768*250);

int rotX2 = round(filterX2()/32768*250);
int rotY2 = round(filterY2()/32768*250);
int rotZ2 = round(filterZ2()/32768*250);

int modX = abs(rotX1 - rotX2);
int modY = abs(rotY1 - rotY2);
int modZ = abs(rotZ1 - rotZ2);

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

lcd.setCursor(0,1);
dtostrf(modX, -1, 0, bufer);
print(bufer, kSize);
lcd.setCursor(4,1);
lcd.print("|");
dtostrf(modY, -1, 0, bufer);
print(bufer, kSize);
lcd.setCursor(9,1);
lcd.print("|");
dtostrf(modZ, -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("dX  |dY  |dZ ");
  mpu1.initialize();
  mpu2.initialize();
  Serial.begin(9600);
  //проверка соединения
  //Serial.println(mpu1.testConnection() ? "MPU6050-1 OK" : "MPU6050-1 FAIL");
  //Serial.println(mpu2.testConnection() ? "MPU6050-2 OK" : "MPU6050-2 FAIL");
}

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