#include <MPU6050.h>  // Libreria per interfacciare il sensore MPU-6050

MPU6050 mpu;  // Crea un oggetto MPU6050

void setup() {
  Serial.begin(115200);  // Inizializza la comunicazione seriale a 115200 baud
  Wire.begin();  // Inizializza l'interfaccia I2C per comunicare con il sensore
  
  mpu.initialize();  // Inizializza il sensore MPU-6050
  if (!mpu.testConnection()) {  // Verifica la connessione con il sensore
    Serial.println("Connessione al MPU6050 fallita");  // Stampa messaggio di errore
    while (1);  // Ciclo infinito in caso di errore
  } else {
    Serial.println("MPU6050 connesso correttamente");  // Conferma la connessione
  }
}

void loop() {
  // Variabili per memorizzare le letture dell'accelerometro
  int16_t accelX_raw, accelY_raw, accelZ_raw;
  
  // Legge i valori dell'accelerometro e li memorizza nelle variabili
  mpu.getAcceleration(&accelX_raw, &accelY_raw, &accelZ_raw);
  
  // Converte i valori grezzi dell'accelerometro in unità di gravità (g)
  float accelX = accelX_raw / 16384.0;  // Scala di ±2g per 16384 unità/g
  float accelY = accelY_raw / 16384.0;
  float accelZ = accelZ_raw / 16384.0;

    // Stampa di debug per visualizzare i valori letti dall'accelerometro
  Serial.print("Valori accelerometro (in g): \n");
  Serial.print("Accel X: "); Serial.print(accelX); Serial.print(" g\n");
  Serial.print("Accel Y: "); Serial.print(accelY); Serial.print(" g\n");
  Serial.print("Accel Z: "); Serial.print(accelZ); Serial.println(" g\n");

// OFFSET PER COMPENSARE IL FATTO CHE IL SENSORE DEBBA ESSERE MESSO IN VERTICALE, OVVERO 90° SU ASSE Z
 float offset = 0;

  // Calcola l'angolo di inclinazione rispetto a ciascun asse
float angoloX = (atan(accelX / sqrt(pow(accelY, 2) + pow(accelZ, 2))) * 180 / PI);  // Angolo rispetto all'asse X
float angoloY = (atan(accelY / sqrt(pow(accelX, 2) + pow(accelZ, 2))) * 180 / PI);  // Angolo rispetto all'asse Y
float angoloZ = (atan(accelZ / sqrt(pow(accelX, 2) + pow(accelY, 2))) * 180 / PI) - offset;  // Angolo rispetto all'asse Z con offset

// Stampa di debug per visualizzare gli angoli di inclinazione
Serial.print(" Angoli di inclinazione (in gradi): \n");
Serial.print(" Angolo X: "); Serial.print(abs(angoloX)); Serial.print("°\n");
Serial.print(" Angolo Y: "); Serial.print(abs(angoloY)); Serial.print("°\n");
Serial.print(" Angolo Z: "); Serial.print(abs(angoloZ)); Serial.print("°\n ");

// Determina la posizione del sensore in base agli angoli
String posizioneSensore;

// MODULO MESSO IN POSIZIONE ORIZZONTALE: XYZ ≈ 0,0,90
// MODULO MESSO IN POSIZIONE VERTICALE: XYZ ≈ 0,0,0
if (abs(angoloZ) > 60 && abs(angoloX) < 10 && abs(angoloY) < 30) {
    posizioneSensore = "Verticale (perfettamente in piedi)";  // Sensore in piedi

// MODULO: XYZ ≈ 30-60,0,30-60
} else if ((abs(angoloZ) > 30 && abs(angoloZ) < 60) || (abs(angoloX) > 30 && abs(angoloX) < 60) || (abs(angoloY) > 30 && abs(angoloY) < 60)) {
    posizioneSensore = "Sensore inclinato da 30° a 60°";  // Sensore inclinato da 30° a 60° circa

// MODULO: XYZ ≈ 90,0,0 
} else if (abs(angoloZ) < 30 || abs(angoloX) > 60) {
    posizioneSensore = "Coricato a terra";  // Sensore coricato da 0° a 30° 

} else {
    posizioneSensore = "-- posizione non contemplata --";
}


// Stampa la posizione del sensore
Serial.print("Posizione del sensore: "); Serial.println(posizioneSensore);

  // Attendere 
  delay(2000);
}