#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Wire.h>
#include <MPU6050.h>

// Пин-конфигурация для ILI9341
#define TFT_CS   10
#define TFT_DC    9
#define TFT_RST   8

Adafruit_ILI9341 tft(TFT_CS, TFT_DC, TFT_RST);
MPU6050 mpu;

// Калибровочные параметры (настраивайте под ваш датчик)
float accelOffsetX = 0;
float accelOffsetY = 0;
float accelOffsetZ = 0;

void setup() {
  tft.begin(); // Инициализация дисплея
  tft.setRotation(3); // Портретный режим (настройте под ваше крепление)
  tft.fillScreen(ILI9341_WHITE);

  Wire.begin();
  mpu.initialize();
  
  if (!mpu.testConnection()) {
    tft.setTextColor(ILI9341_RED);
    tft.setTextSize(2);
    tft.println("MPU6050 Error!");
    while(1);
  }
}

void loop() {
  // Чтение сырых данных
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Нормализация данных
  float accelX = (ax / 16384.0) - accelOffsetX;
  float accelY = (ay / 16384.0) - accelOffsetY;
  float accelZ = (az / 16384.0) - accelOffsetZ;

  // Вычисление углов
  float roll = atan2(accelY, accelZ) * RAD_TO_DEG;
  float pitch = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * RAD_TO_DEG;

  // Коэффициенты чувствительности
  int deltaY = pitch * 2.0;    // Вертикальная чувствительность
  int deltaRoll = roll * 3.5;  // Горизонтальная чувствительность

  // Расчет границы жидкости
  int startY = 120 + deltaY - deltaRoll; // Центр экрана 240/2=120
  int endY = 120 + deltaY + deltaRoll;

  // Ограничение значений
  startY = constrain(startY, 0, 240);
  endY = constrain(endY, 0, 240);

  // Отрисовка
  tft.fillScreen(ILI9341_WHITE);
  
  for(int x = 0; x < tft.width(); x++) {
    int y = map(x, 0, tft.width()-1, startY, endY);
    y = constrain(y, 0, tft.height());
    tft.drawFastVLine(x, y, tft.height() - y, ILI9341_BLUE);
  }

  delay(20); // Оптимизированная задержка
}