#include "sensors_handle.h" // подключение библиотеки

SensorMotion sensor_1(0x68);
SensorMotion sensor_2(0x69);

float m1[] = {0, 0, 0, 0, 0, 0}; // результаты измерения
float m2[] = {0, 0, 0, 0, 0, 0}; // результаты измерения
float size = 6;

float dm[] = {0, 0, 0, 0, 0, 0};

#include <LiquidCrystal.h> 
// lcd
LiquidCrystal lcd(13, 12, 11, 10, 9, 8);
const int LCD_ROWS = 4;
const int LCD_COLS = 20;


void setup() {
  lcd.begin(LCD_COLS, LCD_ROWS);
  lcd.clear();
  lcd.print(" daX    daY    daZ  ");
  lcd.setCursor(0,2);
  lcd.print(" dgX    dgY    dgZ  ");

  Serial.begin(9600); // дублирование выхода на COM порт

  sensor_1.initialize();
  sensor_2.initialize();
  sensor_1.add_moving_average(10);
  sensor_2.add_moving_average(10);
  Serial.println(sensor_1.test_connection() ? "sensor_1 MPU6050 OK" : "sensor_1 MPU6050 FAIL"); // состояние соединения
  Serial.println(sensor_2.test_connection() ? "sensor_2 MPU6050 OK" : "sensor_2 MPU6050 FAIL"); // состояние соединения
  delay(1000);
}

void UpdateScreen() {
  lcd.setCursor(0,1);
  lcd.print("                ");

  int pos = 0;
  for (int i = 0; i < 3; ++i) {
    lcd.setCursor(pos, 1);
    lcd.print(dm[i]);
    pos += 7;
  }

  lcd.setCursor(0, 3);
  lcd.print("                ");
    
  pos = 0;
  for (int i = 3; i < 6; ++i) {
    lcd.setCursor(pos, 3);
    lcd.print(dm[i]);
    pos += 7;
  }
}

void ReadData() {
  sensor_1.get_motion6(m1[0], m1[1], m1[2], m1[3], m1[4], m1[5]);
  sensor_2.get_motion6(m2[0], m2[1], m2[2], m2[3], m2[4], m2[5]);
}

void CalculateDifference() {
  for (int i = 0; i < size; ++i) {
    dm[i] = abs(m1[i] - m2[i]);
  }
}

void loop() {
  ReadData();
  CalculateDifference();
  UpdateScreen();
  delay(100);
}