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

const int MPU6050Address = 0x68;
LiquidCrystal display(12, 11, 10, 9, 8, 7);

int16_t accelerationReadings[3];
float averagedAcceleration[3] = {0, 0, 0};
const int measurementCount = 10;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU6050Address);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  display.begin(16, 2);
}

void loop() {
  static float accumulatedValues[3] = {0, 0, 0};
  static int readCount = 0;

  fetchSensorData(accelerationReadings);

  for (int i = 0; i < 3; i++) {
    accumulatedValues[i] += accelerationReadings[i];
  }
  readCount++;

  if (readCount == measurementCount) {
    for (int i = 0; i < 3; i++) {
      averagedAcceleration[i] = accumulatedValues[i] / measurementCount;
      accumulatedValues[i] = 0;
    }
    readCount = 0;

    calculateAndShowAngles(averagedAcceleration);
  }

  delay(100);
}

void fetchSensorData(int16_t *sensorData) {
  Wire.beginTransmission(MPU6050Address);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050Address, 14, true);
  for (int i = 0; i < 3; i++) {
    sensorData[i] = Wire.read() << 8 | Wire.read();
  }
}

void calculateAndShowAngles(float *accelData) {
  float xAccel = accelData[0];
  float yAccel = accelData[1];
  float zAccel = accelData[2];

  float tiltX = atan(xAccel / sqrt(yAccel * yAccel + zAccel * zAccel)) * 180.0 / PI;
  float tiltY = atan(yAccel / sqrt(xAccel * xAccel + zAccel * zAccel)) * 180.0 / PI;

  display.clear();
  display.setCursor(0, 0);
  display.print("X: ");
  display.print(tiltX);
  display.setCursor(0, 1);
  display.print("Y: ");
  display.print(tiltY);
}