#include <Wire.h>
#include <MPU6050_tockn.h>
#include <Kalman.h>

#define DEBUG

#ifdef DEBUG
#define dprint(...) Serial.print(__VA_ARGS__)
#define dprintln(...) Serial.println(__VA_ARGS__)
#else
#define dprint(...)
#define dprintln(...)
#endif

MPU6050 mpu6050(Wire);

Kalman kalmanX; // Kalman filter for the X axis
Kalman kalmanY; // Kalman filter for the Y axis
Kalman kalmanZ; // Kalman filter for the Z axis

// Variables to store the sum of the sensor readings
int16_t accelXSum = 0, accelYSum = 0, accelZSum = 0;
int16_t gyroXSum = 0, gyroYSum = 0, gyroZSum = 0;

// Variables to store the average sensor readings
float accelXAvg, accelYAvg, accelZAvg;
float gyroXAvg, gyroYAvg, gyroZAvg;

void setup()
{
  Serial.begin(115200);
  dprintln("Program started: ");
  mpu6050.begin();

  // Start the 2-minute calibration period
  unsigned long startTime = millis();
  while (millis() - startTime < 120000)
  {
    // Read the sensor values
    mpu6050.update();
    int16_t accelX = mpu6050.getAccX();
    int16_t accelY = mpu6050.getAccY();
    int16_t accelZ = mpu6050.getAccZ();
    int16_t gyroX = mpu6050.getGyroX();
    int16_t gyroY = mpu6050.getGyroY();
    int16_t gyroZ = mpu6050.getGyroZ();

    // Add the sensor values to the sum
    accelXSum += accelX;
    accelYSum += accelY;
    accelZSum += accelZ;
    gyroXSum += gyroX;
    gyroYSum += gyroY;
    gyroZSum += gyroZ;

    delay(10);
  }

  // Calculate the average sensor values
  int sampleCount = 120000 / 10;
  accelXAvg = (float)accelXSum / sampleCount;
  accelYAvg = (float)accelYSum / sampleCount;
  accelZAvg = (float)accelZSum / sampleCount;
  gyroXAvg = (float)gyroXSum / sampleCount;
  gyroYAvg = (float)gyroYSum / sampleCount;
  gyroZAvg = (float)gyroZSum / sampleCount;

  dprintln("Dati per le correzioni:");
  dprint("Accellerometro asse x: ");
  dprintln(accelXAvg);
  dprint("Accellerometro asse y: ");
  dprintln(accelYAvg);
  dprint("Accellerometro asse z: ");
  dprintln(accelZAvg);
  dprint("Giroscopio asse x: ");
  dprintln(gyroXAvg);
  dprint("Giroscopio asse y: ");
  dprintln(gyroYAvg);
  dprint("Giroscopio asse z: ");
  dprintln(gyroZAvg);


  // Initialize the Kalman filters
  kalmanX.setAngle(accelXAvg);
  kalmanY.setAngle(accelYAvg);
  kalmanZ.setAngle(accelZAvg);
}

void loop()
{
  mpu6050.update();

  // Get the raw sensor values
  int16_t accelX = mpu6050.getAccX();
  int16_t accelY = mpu6050.getAccY();
  int16_t accelZ = mpu6050.getAccZ();
  int16_t gyroX = mpu6050.getGyroX();
  int16_t gyroY = mpu6050.getGyroY();
  int16_t gyroZ = mpu6050.getGyroZ();

  // Apply the Kalman filter to the sensor values
  float filteredAccelX = kalmanX.getAngle(accelX, gyroX, 0.01);
  float filteredAccelY = kalmanY.getAngle(accelY, gyroY, 0.01);
  float filteredAccelZ = kalmanZ.getAngle(accelZ, gyroZ, 0.01);

  // Print the filtered sensor values
  Serial.print("Filtered Accelerometer X: ");
  Serial.println(filteredAccelX);
  Serial.print("Filtered Accelerometer Y: ");
  Serial.println(filteredAccelY);
  Serial.print("Filtered Accelerometer Z: ");
  Serial.println(filteredAccelZ);

  delay(10);
}