#include <Wire.h>             // Libreria standard per la comunicazione I2C
#include <Adafruit_MPU6050.h> // Libreria per il sensore MPU6050 (disponibile in Wokwi)
#include <Adafruit_Sensor.h>
// #include <QMC5883LCompass.h> // Libreria per la bussola --> Bussola non presente nel simulatore
#include <math.h>

Adafruit_MPU6050 mpu;
// Definizioni dei Pin per gli Encoder
#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3
#define ENCODER2_A_PIN 4
#define ENCODER2_B_PIN 5
#define ENCODER3_A_PIN 6
#define ENCODER3_B_PIN 7

// Contatori degli impulsi, 'volatile' perché modificato in interrupt
volatile long encoderTicks = 0; 
volatile long encoderTicks2 = 0;
volatile long encoderTicks3 = 0;

// Costanti fisiche e di calcolo
#define CPR 1320.0        // Counts Per Revolution (Impulsi per Giro)
#define SAMPLE_TIME_MS 250 // Intervallo di tempo per il calcolo (100 ms)

// Variabili per il calcolo della velocità dell'Encoder 1
volatile long prevEncoderTicks1 = 0; // Ticks letti nell'ultima misurazione
float omega1 = 0.0;                  // Velocità angolare in rad/s
unsigned long prevTime = 0;          // Tempo dell'ultima misurazione (per il loop)

// Variabili per il calcolo della velocità dell'Encoder 2
volatile long prevEncoderTicks2 = 0;
float omega2 = 0.0; 

// Variabili per il calcolo della velocità dell'Encoder 3
volatile long prevEncoderTicks3 = 0;
float omega3 = 0.0;

// QMC5883LCompass compass;
// float current_heading = 0.0; // Angolo di Heading (0-360 gradi)

// Costanti geometriche del robot (da misurare sul modello reale)
#define WHEEL_RADIUS 0.0325    // raggio della ruota in metri (es. 32.5 mm)
#define DISTANCE_TO_CENTER 0.1 // L - distanza dal centro del robot a ciascuna ruota in metri (es. 100 mm)

// Angoli di montaggio delle ruote in radianti
// Assumiamo che le ruote siano montate a 120 gradi l'una dall'altra
// Ruota 1: Avanti, 90 gradi (pi/2 rad)
// Ruota 2: Indietro a sinistra, 210 gradi (7*pi/6 rad)
// Ruota 3: Indietro a destra, 330 gradi (11*pi/6 rad)
#define ANGLE1 PI/2.0        // Ruota 1: 90 gradi
#define ANGLE2 (7.0*PI)/6.0  // Ruota 2: 210 gradi
#define ANGLE3 (11.0*PI)/6.0 // Ruota 3: 330 gradi

// Coefficienti del Filtro Complementare (Per Roll/Pitch, ma utile come concetto)
// Scegliamo Alpha (peso dato al Gyro) molto alto (0.98) per dare prevalenza al Gyro
#define ALPHA 0.98 

// Variabile per la correzione del Gyro (Calibrazione del Gyro Z)
float gyro_bias_z = 0.0; // Usato per calibrare il Gyro Z a zero quando fermo

// Variabili di Stato del Robot (Odometria)
float currentX = 0.0; // Posizione X (metri)
float currentY = 0.0; // Posizione Y (metri)
float currentTheta = 0.0; // Angolo di Heading (radianti)

// Variabili globali per il controllo
char command = ' '; // Inizializza con vuoto per stare fermo all'avvio
const float MAX_OMEGA = 5.0; // Velocità angolare massima di simulazione (rad/s)

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200); // Inizializza comunicazione seriale
    
    if (!mpu.begin()) {
      Serial.println("Errore, MPU6050 non trovato!");
      while (1); // Ferma l'esecuzione in caso di errore
    }
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G); // Imposta l'accelerometro
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);     // Imposta il giroscopio
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);  // Imposta il filtro
    Serial.println("MPU6050 avviato con successo.");

  pinMode(ENCODER_A_PIN, INPUT_PULLUP);
  pinMode(ENCODER_B_PIN, INPUT_PULLUP);

  // Collega la routine di lettura all'interrupt 0 (Pin 2)
  // Cambia il conteggio ad ogni cambio di stato (RISING/FALLING/CHANGE)
  attachInterrupt(digitalPinToInterrupt(ENCODER_A_PIN), readEncoder, CHANGE); 

  // INIZIALIZZAZIONE ENCODER 2 e 3 (Polling)
  pinMode(ENCODER2_A_PIN, INPUT_PULLUP);
  pinMode(ENCODER2_B_PIN, INPUT_PULLUP);
  pinMode(ENCODER3_A_PIN, INPUT_PULLUP);
  pinMode(ENCODER3_B_PIN, INPUT_PULLUP);
  
  Serial.println("Calibrazione Giroscopio Z...");
    
    // Calcola la media di 100 letture per trovare il bias
    // sensors_event_t g;
    sensors_event_t a, g, temp;
    float sum_gyro_z = 0.0;
    
    for (int i = 0; i < 100; i++) {
        // mpu.getGyroSensor(&g);
        mpu.getEvent(&a, &g, &temp);
        sum_gyro_z += g.gyro.z;
        delay(10); 
    }
    
    gyro_bias_z = sum_gyro_z / 100.0;
    Serial.print("Gyro Bias Z Calcolato: ");
    Serial.print(gyro_bias_z, 4);
    Serial.println(" rad/s");

  Serial.println("Encoders avviati con successo.");

  // Wire.begin(); // Assicurati che l'I2C sia avviato per la bussola
  //  compass.init(); // Inizializza la bussola
  //  // Opzionale: Calibrazione o impostazione della modalità
  //  // compass.setCalibration(-645, 786, -690, 680, -29, 21);
    
  //  Serial.println("Bussola avviata con successo.");
}

void loop() {
  checkSerialInput();
  unsigned long currentTime = millis();
  
  // ----------------------------------------------------
  // BLOCCO DI CALCOLO E ODOMETRIA (Eseguito solo ogni 100 ms)
  // ----------------------------------------------------
  if (currentTime - prevTime >= SAMPLE_TIME_MS) {
    
    // 1. Lettura Sensori in Testa al Blocco
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    
    // 2. Lettura Polling Encoder 2 & 3
    // Eseguiamo il polling (meno preciso ma necessario su questi pin)
    if (digitalRead(ENCODER2_A_PIN) != 0) {
      readEncoder2();
    }
    if (digitalRead(ENCODER3_A_PIN) != 0) {
      readEncoder3();
    }

    // 3. Calcolo Base (Delta Tempo)
    float deltaTime_s = (currentTime - prevTime) / 1000.0;

    // --- IMPOSTAZIONE DELLE VELOCITÀ DELLE RUOTE TRAMITE WASD ---

    // Resetta le variabili di simulazione a zero
    float target_omega1 = 0.0;
    float target_omega2 = 0.0;
    float target_omega3 = 0.0;

    switch (command) {
        case 'W': // Avanti (Velocità Y positiva)
            // Per il movimento in avanti, tutte le ruote spingono nella stessa direzione (Y)
            target_omega1 = 0.0;        // La ruota davanti non gira (trascinata)
            target_omega2 = -MAX_OMEGA; // Spinge in avanti
            target_omega3 = MAX_OMEGA;  // Spinge in avanti
            break;
            
        case 'S': // Indietro (Velocità Y negativa)
            target_omega1 = 0.0;
            target_omega2 = MAX_OMEGA;
            target_omega3 = -MAX_OMEGA;
            break;
            
        case 'A': // Rotazione Anti-oraria (Yaw negativo)
            // Per la rotazione pura, tutte le ruote devono ruotare alla stessa velocità angolare
            target_omega1 = MAX_OMEGA;
            target_omega2 = MAX_OMEGA;
            target_omega3 = MAX_OMEGA;
            break;
            
        case 'D': // Rotazione Oraria (Yaw positivo)
            target_omega1 = -MAX_OMEGA;
            target_omega2 = -MAX_OMEGA;
            target_omega3 = -MAX_OMEGA;
            break;
            
        case 'Q': // Traslazione Sinistra (Per testare il movimento laterale)
            // La Cinematica Laterale è più complessa e dipende dagli angoli. 
            // Assumeremo un caso semplice per il test.
            target_omega1 = -MAX_OMEGA;
            target_omega2 = MAX_OMEGA;
            target_omega3 = -MAX_OMEGA;
            break;
            
        default: // Qualsiasi altro tasto ferma il robot (Stop)
            target_omega1 = 0.0;
            target_omega2 = 0.0;
            target_omega3 = 0.0;
            break;
    }

    // Qui dovresti usare target_omega1, target_omega2, target_omega3
    // per impostare i Delta Ticks e aggiornare omega1, omega2, omega3.
    // Poiché stiamo simulando, impostiamo direttamente le variabili omega.
    // NOTA: Questo bypassa completamente la lettura degli encoder per il test WASD.
    omega1 = target_omega1;
    omega2 = target_omega2;
    omega3 = target_omega3;

    // --- STAMPA VELOCITÀ ---
    Serial.print(" | Omega 1/2/3: "); 
    Serial.print(omega1, 3); Serial.print(" / ");
    Serial.print(omega2, 3); Serial.print(" / ");
    Serial.print(omega3, 3); Serial.print(" rad/s");
    
    // 5. Cinematica Inversa (Vx, Vy, V_omega del robot)
    // Trasformazione delle velocità angolari delle ruote (omega) nelle velocità del robot (Vx, Vy, V_omega)
    float Vx_robot_local = (WHEEL_RADIUS / 3.0) * (
        (-sin(ANGLE1) * omega1) + (-sin(ANGLE2) * omega2) + (-sin(ANGLE3) * omega3)
    );
    float Vy_robot_local = (WHEEL_RADIUS / 3.0) * (
        (cos(ANGLE1) * omega1) + (cos(ANGLE2) * omega2) + (cos(ANGLE3) * omega3)
    );

    // V_omega calcolato da encoders. Non lo usiamo per l'aggiornamento,
    // ma serve per diagnostica e controllo.
    float V_omega_robot_enc = (WHEEL_RADIUS / (3.0 * DISTANCE_TO_CENTER)) * (omega1 + omega2 + omega3); 

    // 6. Trasformazione Globale (Usando currentTheta)
    float cos_h = cos(currentTheta);
    float sin_h = sin(currentTheta);
    float Vx_global = (Vx_robot_local * cos_h) - (Vy_robot_local * sin_h);
    float Vy_global = (Vx_robot_local * sin_h) + (Vy_robot_local * cos_h);

    // 7. Aggiornamento Posizione (Odometria)
    currentX += Vx_global * deltaTime_s;
    currentY += Vy_global * deltaTime_s;
    
    // Aggiornamento Angolo Theta (Utilizzo del Gyro Z per la Rotazione)
    // Gyro Z Corretto (sottrai il bias calcolato)
    float corrected_gyro_z = g.gyro.z - gyro_bias_z;

    // Questo è il principio di integrazione Gyro, più stabile del calcolo Cinematico
    // per la stima dell'angolo (ma soggetto a drift).
    // currentTheta += g.gyro.z * deltaTime_s; 

    // Questo deve usare la velocità calcolata dagli encoder:
    currentTheta += V_omega_robot_enc * deltaTime_s;

    // Normalizzazione dell'angolo Theta (-PI a PI)
    currentTheta = fmod(currentTheta, 2.0 * PI);
    if (currentTheta > PI) currentTheta -= 2.0 * PI;
    if (currentTheta < -PI) currentTheta += 2.0 * PI;

    // --- STAMPA ODOMETRIA ---
    Serial.print(" | Posizione (X, Y, Theta): ");
    Serial.print(currentX, 3); Serial.print(", ");
    Serial.print(currentY, 3); Serial.print(", ");
    Serial.print(currentTheta * 180.0 / PI, 1); Serial.print(" deg");
    
    // --- STAMPA IMU ---
    Serial.print(" | Gyro Z (MPU): "); Serial.print(g.gyro.z, 3); 
    Serial.print(" rad/s");

    // --- STAMPA DIAGNOSTICA ---
    Serial.print(" | Accel Z: "); Serial.print(a.acceleration.z, 2); 
    Serial.print(" m/s^2");

    // 8. Aggiornamento Tempo
    prevTime = currentTime; 
  }
}

void readEncoder() {
  // Legge lo stato del Pin B.
  // Se Pin B è ALTO quando A è innescato, sta ruotando in avanti.
  if (digitalRead(ENCODER_B_PIN) == HIGH) {
    encoderTicks++; // In avanti
  } else {
    encoderTicks--; // All'indietro
  }
}

// Funzione di Polling per l'Encoder 2
void readEncoder2() {
  if (digitalRead(ENCODER2_B_PIN) == HIGH) {
    encoderTicks2++;
  } else {
    encoderTicks2--;
  }
}

// Funzione di Polling per l'Encoder 3
void readEncoder3() {
  if (digitalRead(ENCODER3_B_PIN) == HIGH) {
    encoderTicks3++;
  } else {
    encoderTicks3--;
  }
}

// Funzione per leggere l'input seriale
void checkSerialInput() {
  if (Serial.available()) {
    command = Serial.read();
    
    // Converti l'input a maiuscolo per uniformità
    if (command >= 'a' && command <= 'z') {
      command = command - ('a' - 'A');
    }
  }
}