/*
 * ============================================================================
 *  ROBOT AUTO-ÉQUILIBRÉ — Arduino MKR Zero + MPU6050 + 2 ponts en H
 * ============================================================================
 *
 *  PRINCIPE GÉNÉRAL
 *  ----------------
 *  1. On lit le capteur MPU6050 (accéléromètre + gyroscope) par I2C.
 *  2. Un filtre de Kalman fusionne les deux mesures pour obtenir un angle
 *     fiable : le gyroscope est précis à court terme mais dérive, et
 *     l'accéléromètre est juste à long terme mais bruité.
 *  3. Un correcteur PID compare cet angle à la consigne (angle d'équilibre)
 *     et calcule une commande.
 *  4. Cette commande est convertie en PWM pour les deux moteurs.
 *
 *  DÉROULEMENT DU PROGRAMME (machine à états)
 *  ------------------------------------------
 *  ERREUR      -> capteur introuvable : nouvel essai toutes les secondes.
 *  CALIBRATION -> ~1 s au démarrage : NE PAS BOUGER le robot.
 *  ATTENTE     -> moteurs coupés tant que l'angle n'est pas proche de la
 *                 consigne (± 5°). Redresser le robot à la main.
 *  ÉQUILIBRE   -> la régulation PID pilote les moteurs.
 *                 Si le robot tombe (> 35°), retour en ATTENTE.
 *
 *  ATTENTION : après une chute, les moteurs redémarrent AUTOMATIQUEMENT
 *  dès que le robot est ramené près de la consigne. Tenir le robot
 *  fermement et garder les doigts loin des roues.
 *
 *  LED INTÉGRÉE
 *  ------------
 *  Clignotement rapide : ERREUR (vérifier le câblage du MPU6050)
 *  Éteinte             : CALIBRATION en cours
 *  Clignotement lent   : ATTENTE (redresser le robot)
 *  Allumée fixe        : ÉQUILIBRE (régulation active)
 *
 *  MONITEUR SÉRIE (115200 bauds)
 *  -----------------------------
 *  Ouvrir le moniteur série pour suivre les messages et, toutes les
 *  200 ms, l'état, l'angle filtré, la mesure brute et la commande.
 *  Si l'angle affiché ne bouge pas quand on penche le robot, changer
 *  AXE_ROTATION. Si l'angle au repos est loin de 8°, ajuster la consigne.
 *
 *  RÉGLAGES À VÉRIFIER SUR LE ROBOT
 *  --------------------------------
 *  - AXE_ROTATION : axe du capteur autour duquel le robot bascule.
 *  - SENS_MOTEUR1 / SENS_MOTEUR2 : inverser (-1) si une roue tourne à
 *    l'envers ou si le robot accélère sa chute au lieu de la corriger.
 *  - consigne, Kp, Ki, Kd, OFFSET_PWM, RATIO_MOTEUR2 : à ajuster par essais.
 * ============================================================================
 */

#include <Arduino.h>
#include <Wire.h>
#include <math.h>

// ============================================================================
//  1. CONFIGURATION MATÉRIELLE
// ============================================================================

// Le MPU6050 répond à l'adresse 0x68 (broche AD0 à la masse) ou 0x69
// (AD0 à 3,3 V). Le programme essaie automatiquement les deux.
uint8_t adresse_mpu = 0x68;

// Axe de rotation du robot, vu par le capteur : 0 = X, 1 = Y, 2 = Z.
// Le code d'origine calculait atan2(accel_x, accel_y) : cette formule
// mesure une rotation autour de Z, donc le gyroscope Z doit être utilisé.
const uint8_t AXE_ROTATION = 2;

// Broches des ponts en H (toutes compatibles PWM sur MKR Zero).
// Logique du driver : IN1 = PWM / IN2 = 0 -> avance
//                     IN1 = 0 / IN2 = PWM -> recule
const uint8_t IN1_MOT1 = 2;
const uint8_t IN2_MOT1 = 3;
const uint8_t IN1_MOT2 = 4;
const uint8_t IN2_MOT2 = 5;

// Mettre -1 pour inverser le sens d'un moteur sans recâbler.
const int SENS_MOTEUR1 = 1;
const int SENS_MOTEUR2 = 1;

// ============================================================================
//  2. PARAMÈTRES DES MOTEURS
// ============================================================================

const int PWM_MAX = 255;               // PWM sur 8 bits : 0 à 255
const int OFFSET_PWM = 0;             // PWM minimale pour vaincre la friction
const float RATIO_MOTEUR2 = 1.0f;     // Le moteur 2 est bridé à 85 %

// La commande PID est limitée pour que OFFSET_PWM + commande <= 255.
// Sans cette limite, l'offset pouvait faire dépasser 255.
const float LIMITE_COMMANDE = PWM_MAX - OFFSET_PWM;

// En dessous de cette commande, on coupe les moteurs (évite les à-coups
// dus à l'offset quand le robot est déjà presque à l'équilibre).
const float ZONE_MORTE = 0.5f;

// ============================================================================
//  3. PARAMÈTRES DE TEMPS ET DE SÉCURITÉ
// ============================================================================

const uint32_t PERIODE_US = 5000;      // Boucle de régulation à 200 Hz
const uint32_t DT_MAX_US = 20000;      // dt limité à 20 ms en cas de retard
const uint32_t PERIODE_AFFICHAGE_MS = 200;

const float ANGLE_DEMARRAGE = 10.0f;    // Démarrage si |erreur| < 5°
const float ANGLE_CHUTE = 45.0f;       // Arrêt si |erreur| > 35°

// Conversions du MPU6050 (calibres par défaut, configurés dans le setup).
const float DEGRES_PAR_RADIAN = 180.0f / PI;
const float GYRO_LSB_PAR_DPS = 131.0f;    // Gyro ±250 °/s
const float ACCEL_LSB_PAR_G = 16384.0f;   // Accéléromètre ±2 g

// ============================================================================
//  4. FILTRE DE KALMAN ET PID
// ============================================================================

// Kalman : Q = confiance dans le modèle (gyro), R = bruit de l'accéléromètre.
const float Q_angle = 0.001f;
const float Q_bias = 0.003f;
const float R_measure = 0.1f;

// Gains PID (valeurs d'origine, à ajuster sur le robot).
const float Kp = 125.0f; //100.0f
const float Ki = 2000.0f;//1.0f;
const float Kd = 2.0f; //2.5f

float consigne = 7.73f;                 // Angle d'équilibre souhaité, en degrés

// ============================================================================
//  5. VARIABLES GLOBALES
// ============================================================================

// Valeurs brutes du capteur
int16_t accel_x = 0, accel_y = 0, accel_z = 0;
int16_t gyro_x = 0, gyro_y = 0, gyro_z = 0;
float temperature = 0.0f;

// État du filtre de Kalman
float angle = 0.0f;      // Angle estimé (degrés)
float bias = 0.0f;       // Dérive estimée du gyroscope (°/s)
float rate = 0.0f;       // Vitesse angulaire corrigée (°/s)
float P[2][2] = {{R_measure, 0.0f}, {0.0f, 0.1f}};

// État du PID
float integrale = 0.0f;  // Terme intégral, directement en unités PWM
float commande = 0.0f;   // Dernière commande envoyée (pour l'affichage)
float derniere_mesure = 0.0f;

// Gestion du temps
uint32_t dernier_cycle_us = 0;
uint32_t dernier_echantillon_us = 0;
uint32_t dernier_affichage_ms = 0;
uint32_t dernier_essai_ms = 0;

// Machine à états
const uint8_t ETAT_ERREUR = 0;
const uint8_t ETAT_CALIBRATION = 1;
const uint8_t ETAT_ATTENTE = 2;
const uint8_t ETAT_EQUILIBRE = 3;
uint8_t etat = 255;      // 255 = pas encore initialisé

// ============================================================================
//  6. FONCTIONS UTILITAIRES
// ============================================================================

float borner(float valeur, float minimum, float maximum) {
  if (valeur < minimum) return minimum;
  if (valeur > maximum) return maximum;
  return valeur;
}

// Ramène un angle dans [-180°, 180°]. Sans cela, passer de +179° à -179°
// serait vu comme un saut de 358° au lieu de 2°.
float normaliserAngle(float valeur) {
  return remainderf(valeur, 360.0f);
}

const __FlashStringHelper *nomEtat(uint8_t e) {
  if (e == ETAT_ERREUR) return F("ERREUR");
  if (e == ETAT_CALIBRATION) return F("CALIBRATION");
  if (e == ETAT_ATTENTE) return F("ATTENTE");
  if (e == ETAT_EQUILIBRE) return F("EQUILIBRE");
  return F("?");
}

// ============================================================================
//  7. MOTEURS
// ============================================================================

void arreterMoteurs() {
  analogWrite(IN1_MOT1, 0);
  analogWrite(IN2_MOT1, 0);
  analogWrite(IN1_MOT2, 0);
  analogWrite(IN2_MOT2, 0);
}

// pwm > 0 : avance, pwm < 0 : recule, pwm = 0 : arrêt.
void appliquerMoteur(uint8_t in1, uint8_t in2, int pwm) {
  // On coupe toujours l'entrée opposée AVANT d'activer l'autre, pour ne
  // jamais avoir IN1 et IN2 actifs en même temps.
  if (pwm > 0) {
    analogWrite(in2, 0);
    analogWrite(in1, pwm);
  } else if (pwm < 0) {
    analogWrite(in1, 0);
    analogWrite(in2, -pwm);
  } else {
    analogWrite(in1, 0);
    analogWrite(in2, 0);
  }
}

void commanderMoteurs(float cmd) {
  if (!isfinite(cmd) || fabsf(cmd) <= ZONE_MORTE) {
    arreterMoteurs();
    return;
  }

  // Le signe donne le sens, la valeur absolue donne la vitesse.
  const int sens = (cmd > 0.0f) ? 1 : -1;
  const float amplitude = fminf(fabsf(cmd), LIMITE_COMMANDE);

  // L'offset est ajouté APRÈS la limitation : la PWM ne dépasse jamais 255
  // et le ratio du moteur 2 reste appliqué même à pleine vitesse.
  const int pwm1 = (int)lroundf(OFFSET_PWM + amplitude);
  const int pwm2 = (int)lroundf(OFFSET_PWM + amplitude * RATIO_MOTEUR2);

  appliquerMoteur(IN1_MOT1, IN2_MOT1, sens * SENS_MOTEUR1 * pwm1);
  appliquerMoteur(IN1_MOT2, IN2_MOT2, sens * SENS_MOTEUR2 * pwm2);
}

// ============================================================================
//  8. MACHINE À ÉTATS, LED ET AFFICHAGE
// ============================================================================

void changerEtat(uint8_t nouvel_etat, const __FlashStringHelper *message) {
  if (nouvel_etat == etat) return;   // Évite de répéter le même message

  // Par sécurité, les moteurs sont coupés dès qu'on quitte la régulation.
  if (nouvel_etat != ETAT_EQUILIBRE) arreterMoteurs();

  // L'intégrale repart de zéro à chaque changement d'état, sinon une
  // erreur accumulée avant une chute provoquerait un à-coup au redémarrage.
  integrale = 0.0f;
  commande = 0.0f;
  etat = nouvel_etat;

  Serial.print(F(">> "));
  Serial.print(nomEtat(etat));
  Serial.print(F(" : "));
  Serial.println(message);
}

void mettreAJourLED() {
  const uint32_t t = millis();
  bool allumee = false;
  if (etat == ETAT_EQUILIBRE) allumee = true;
  else if (etat == ETAT_ATTENTE) allumee = (t / 500) % 2;
  else if (etat == ETAT_ERREUR) allumee = (t / 100) % 2;
  digitalWrite(LED_BUILTIN, allumee ? HIGH : LOW);
}

// Affichage périodique (non bloquant) pour comprendre ce que voit le robot.
void afficherEtat() {
  const uint32_t t = millis();
  if (t - dernier_affichage_ms < PERIODE_AFFICHAGE_MS) return;
  dernier_affichage_ms = t;

  Serial.print(F("etat="));
  Serial.print(nomEtat(etat));
  Serial.print(F("  angle="));
  Serial.print(angle, 2);
  Serial.print(F("  mesure_accel="));
  Serial.print(derniere_mesure, 2);
  Serial.print(F("  consigne="));
  Serial.print(consigne, 2);
  Serial.print(F("  commande="));
  Serial.println(commande, 1);
}

// ============================================================================
//  9. COMMUNICATION I2C AVEC LE MPU6050
// ============================================================================

bool ecrireRegistre(uint8_t registre, uint8_t valeur) {
  Wire.beginTransmission(adresse_mpu);
  Wire.write(registre);
  Wire.write(valeur);
  return Wire.endTransmission(true) == 0;   // 0 = succès
}

// Lit "taille" octets à partir de "registre". Renvoie false si le capteur
// ne répond pas ou envoie moins d'octets que prévu.
bool lireRegistres(uint8_t registre, uint8_t *donnees, size_t taille) {
  Wire.beginTransmission(adresse_mpu);
  Wire.write(registre);
  if (Wire.endTransmission(false) != 0) return false;

  const size_t recus = Wire.requestFrom(adresse_mpu, taille, true);
  if (recus != taille || Wire.available() < (int)taille) {
    while (Wire.available() > 0) Wire.read();   // Vide le tampon
    return false;
  }
  for (size_t i = 0; i < taille; ++i) {
    donnees[i] = (uint8_t)Wire.read();
  }
  return true;
}

// Assemble deux octets (poids fort puis poids faible) en entier signé.
int16_t decoder16(const uint8_t *donnees) {
  const uint16_t brut = ((uint16_t)donnees[0] << 8) | donnees[1];
  return (int16_t)brut;
}

// Lecture des 14 octets de mesure en une seule transaction.
// Remarque : dans le code d'origine, "Wire.read() << 8 | Wire.read()"
// ne garantit pas l'ordre des deux lectures en C++. Ici, les octets
// sont d'abord stockés dans un tableau, puis assemblés dans le bon ordre.
bool lireMPU() {
  uint8_t d[14];
  if (!lireRegistres(0x3B, d, sizeof(d))) return false;
  accel_x = decoder16(d);
  accel_y = decoder16(d + 2);
  accel_z = decoder16(d + 4);
  temperature = decoder16(d + 6) / 340.0f + 36.53f;
  gyro_x = decoder16(d + 8);
  gyro_y = decoder16(d + 10);
  gyro_z = decoder16(d + 12);
  return true;
}

// Cherche le capteur aux adresses 0x68 puis 0x69 et le configure.
bool initialiserMPU() {
  const uint8_t adresses[2] = {0x68, 0x69};
  uint8_t identite = 0;
  bool trouve = false;

  for (uint8_t i = 0; i < 2 && !trouve; ++i) {
    adresse_mpu = adresses[i];
    trouve = lireRegistres(0x75, &identite, 1);   // Registre WHO_AM_I
  }
  if (!trouve) {
    Serial.println(F("MPU6050 introuvable en 0x68 et 0x69. Verifier SDA, SCL, VCC et GND."));
    return false;
  }

  Serial.print(F("Capteur detecte a l'adresse 0x"));
  Serial.print(adresse_mpu, HEX);
  Serial.print(F(", WHO_AM_I = 0x"));
  Serial.println(identite, HEX);

  // Beaucoup de modules vendus comme "MPU6050" sont des copies qui
  // renvoient un autre identifiant : on prévient, mais on continue.
  if (identite != 0x68) {
    Serial.println(F("Attention : identifiant inhabituel (clone ?). On continue."));
  }

  if (!ecrireRegistre(0x6B, 0x80)) return false;   // Réinitialisation
  delay(100);
  if (!ecrireRegistre(0x6B, 0x01) ||   // Réveil, horloge du gyroscope
      !ecrireRegistre(0x6C, 0x00) ||   // Tous les axes actifs
      !ecrireRegistre(0x1B, 0x00) ||   // Gyro ±250 °/s -> 131 LSB par °/s
      !ecrireRegistre(0x1C, 0x00) ||   // Accéléro ±2 g -> 16384 LSB par g
      !ecrireRegistre(0x1A, 0x03) ||   // Filtre passe-bas interne ~44 Hz
      !ecrireRegistre(0x19, 0x04)) {   // Fréquence d'échantillonnage 200 Hz
    Serial.println(F("Echec de configuration du MPU6050."));
    return false;
  }
  delay(100);
  return true;
}

// ============================================================================
//  10. CALCUL DE L'ANGLE
// ============================================================================

// Angle donné par l'accéléromètre : direction de la gravité dans le plan
// perpendiculaire à l'axe de rotation. Renvoie false si la mesure est
// inexploitable (gravité presque alignée avec l'axe de rotation).
bool mesurerAngle(float &mesure) {
  float horizontal, vertical;
  if (AXE_ROTATION == 0) {
    horizontal = accel_y;
    vertical = accel_z;
  } else if (AXE_ROTATION == 1) {
    horizontal = -(float)accel_x;
    vertical = accel_z;
  } else {
    horizontal = accel_x;   // Formule du code d'origine
    vertical = accel_y;
  }
  if (hypotf(horizontal, vertical) < 0.1f * ACCEL_LSB_PAR_G) return false;
  mesure = atan2f(horizontal, vertical) * DEGRES_PAR_RADIAN;
  return isfinite(mesure);
}

// Vitesse de rotation (°/s) autour de l'axe choisi. Elle doit
// correspondre au même axe que mesurerAngle(), sinon le filtre
// mélange deux rotations différentes (erreur du code d'origine,
// qui utilisait gyro_x avec un angle calculé autour de Z).
float vitesseGyro() {
  int16_t brut = gyro_z;
  if (AXE_ROTATION == 0) brut = gyro_x;
  else if (AXE_ROTATION == 1) brut = gyro_y;
  return brut / GYRO_LSB_PAR_DPS;
}

// ============================================================================
//  11. CALIBRATION
// ============================================================================

// Robot immobile pendant ~1 s : on mesure la dérive du gyroscope et
// l'angle de départ. Renvoie false (et explique pourquoi) si le robot a
// bougé : la calibration sera alors relancée automatiquement.
bool calibrer() {
  const uint16_t NB_MESURES = 200;
  float somme_vitesse = 0.0f, somme_sin = 0.0f, somme_cos = 0.0f;
  float vitesse_min = 1e9f, vitesse_max = -1e9f;
  float premier_angle = 0.0f;

  for (uint16_t i = 0; i < NB_MESURES; ++i) {
    if (!lireMPU()) {
      changerEtat(ETAT_ERREUR, F("Lecture I2C impossible pendant la calibration."));
      return false;
    }

    float mesure;
    if (!mesurerAngle(mesure)) {
      Serial.println(F("Calibration : angle inexploitable. Verifier AXE_ROTATION."));
      return false;
    }

    // La norme de l'accélération doit valoir environ 1 g au repos.
    const float norme_g = sqrtf((float)accel_x * accel_x + (float)accel_y * accel_y +
                                (float)accel_z * accel_z) / ACCEL_LSB_PAR_G;
    if (norme_g < 0.8f || norme_g > 1.2f) {
      Serial.print(F("Calibration : acceleration anormale ("));
      Serial.print(norme_g, 2);
      Serial.println(F(" g). Ne pas bouger le robot."));
      return false;
    }

    if (i == 0) premier_angle = mesure;
    if (fabsf(normaliserAngle(mesure - premier_angle)) > 3.0f) {
      Serial.println(F("Calibration : le robot a bouge. Nouvel essai..."));
      return false;
    }

    const float v = vitesseGyro();
    vitesse_min = fminf(vitesse_min, v);
    vitesse_max = fmaxf(vitesse_max, v);
    somme_vitesse += v;

    // Moyenne d'angles via sin/cos pour gérer correctement ±180°.
    somme_sin += sinf(mesure / DEGRES_PAR_RADIAN);
    somme_cos += cosf(mesure / DEGRES_PAR_RADIAN);
    delay(5);
  }

  // On teste la VARIATION du gyroscope et non sa valeur : un capteur
  // bon marché peut avoir une dérive fixe de plusieurs °/s au repos,
  // c'est justement ce que la calibration doit mesurer.
  if (vitesse_max - vitesse_min > 10.0f) {
    Serial.println(F("Calibration : rotation detectee. Ne pas bouger le robot."));
    return false;
  }

  bias = somme_vitesse / NB_MESURES;
  angle = atan2f(somme_sin, somme_cos) * DEGRES_PAR_RADIAN;
  derniere_mesure = angle;
  P[0][0] = R_measure; P[0][1] = 0.0f;
  P[1][0] = 0.0f;      P[1][1] = 0.1f;
  return true;
}

// ============================================================================
//  12. FILTRE DE KALMAN
// ============================================================================

void actualiserKalman(float mesure, float vitesse, float dt) {
  // --- Prédiction : on intègre la vitesse du gyroscope ---
  rate = vitesse - bias;
  angle = normaliserAngle(angle + dt * rate);
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;

  // --- Correction : on recale avec l'angle de l'accéléromètre ---
  const float S = P[0][0] + R_measure;
  const float K0 = P[0][0] / S;
  const float K1 = P[1][0] / S;
  const float innovation = normaliserAngle(mesure - angle);
  angle = normaliserAngle(angle + K0 * innovation);
  bias += K1 * innovation;

  // --- Mise à jour de la covariance ---
  const float P00 = P[0][0];
  const float P01 = P[0][1];
  P[0][0] -= K0 * P00;
  P[0][1] -= K0 * P01;
  P[1][0] -= K1 * P00;
  P[1][1] -= K1 * P01;

  rate = vitesse - bias;   // Vitesse corrigée avec la nouvelle dérive
}

// ============================================================================
//  13. CORRECTEUR PID
// ============================================================================

float calculerPID(float erreur, float dt) {
  // P et D. Le terme dérivé utilise la vitesse du gyroscope (déjà filtrée)
  // au lieu de (erreur - erreur_precedente) / dt : c'est moins bruité et
  // il n'y a plus de division par un dt qui pourrait valoir zéro.
  // Comme erreur = consigne - angle, la dérivée de l'erreur vaut -rate.
  const float terme_PD = Kp * erreur - Kd * rate;

  // I avec anti-emballement (anti-windup) : on n'accumule l'intégrale que
  // si la commande n'est pas saturée, ou si elle aide à sortir de la
  // saturation. Une simple limite à ±400 ne suffisait pas, car la sortie
  // était déjà saturée bien avant.
  const float integrale_candidate =
      borner(integrale + Ki * erreur * dt, -LIMITE_COMMANDE, LIMITE_COMMANDE);
  const float sortie_candidate = terme_PD + integrale_candidate;
  if (fabsf(sortie_candidate) <= LIMITE_COMMANDE ||
      (sortie_candidate > LIMITE_COMMANDE && erreur < 0.0f) ||
      (sortie_candidate < -LIMITE_COMMANDE && erreur > 0.0f)) {
    integrale = integrale_candidate;
  }

  return borner(terme_PD + integrale, -LIMITE_COMMANDE, LIMITE_COMMANDE);
}

// ============================================================================
//  14. SETUP
// ============================================================================

void setup() {
  // Les moteurs sont mis à l'arrêt AVANT toute autre chose.
  pinMode(IN1_MOT1, OUTPUT);
  pinMode(IN2_MOT1, OUTPUT);
  pinMode(IN1_MOT2, OUTPUT);
  pinMode(IN2_MOT2, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  analogWriteResolution(8);   // PWM de 0 à 255
  arreterMoteurs();

  // Sur MKR Zero, le port série passe par l'USB natif : les messages
  // envoyés avant l'ouverture du moniteur sont perdus. On attend donc
  // jusqu'à 3 s qu'il soit ouvert (sans bloquer si le robot est sur batterie).
  Serial.begin(115200);
  const uint32_t debut = millis();
  while (!Serial && millis() - debut < 3000) {
    digitalWrite(LED_BUILTIN, ((millis() / 100) % 2) ? HIGH : LOW);
  }
  Serial.println();
  Serial.println(F("=== Robot auto-equilibre : demarrage ==="));

  Wire.begin();
  Wire.setClock(400000);   // I2C rapide (400 kHz)

  dernier_essai_ms = millis();
  if (initialiserMPU()) {
    changerEtat(ETAT_CALIBRATION, F("Ne pas bouger le robot pendant ~1 s..."));
  } else {
    changerEtat(ETAT_ERREUR, F("Capteur non pret. Nouvel essai chaque seconde."));
  }
}

// ============================================================================
//  15. BOUCLE PRINCIPALE
// ============================================================================

void loop() {
  mettreAJourLED();

  // ----- ÉTAT ERREUR : on réessaie d'initialiser le capteur -----
  if (etat == ETAT_ERREUR) {
    arreterMoteurs();
    if (millis() - dernier_essai_ms >= 1000) {
      dernier_essai_ms = millis();
      if (initialiserMPU()) {
        changerEtat(ETAT_CALIBRATION, F("Capteur OK. Ne pas bouger le robot..."));
      }
    }
    return;
  }

  // ----- ÉTAT CALIBRATION -----
  if (etat == ETAT_CALIBRATION) {
    arreterMoteurs();
    if (calibrer()) {
      Serial.print(F("Calibration OK. Angle initial = "));
      Serial.print(angle, 2);
      Serial.print(F(" deg, derive gyro = "));
      Serial.print(bias, 3);
      Serial.println(F(" deg/s"));
      dernier_cycle_us = micros();
      dernier_echantillon_us = dernier_cycle_us;
      changerEtat(ETAT_ATTENTE, F("Redresser le robot pres de la consigne pour demarrer."));
    } else if (etat == ETAT_CALIBRATION) {
      delay(500);   // Petite pause avant de recommencer
    }
    return;
  }

  // ----- ÉTATS ATTENTE et ÉQUILIBRE : boucle cadencée à 200 Hz -----
  const uint32_t maintenant = micros();
  // La soustraction de deux uint32_t reste correcte même quand micros()
  // repasse à zéro (toutes les ~70 minutes).
  if (maintenant - dernier_cycle_us < PERIODE_US) return;
  dernier_cycle_us = maintenant;

  // 1. Lecture du capteur
  if (!lireMPU()) {
    changerEtat(ETAT_ERREUR, F("Lecture du MPU6050 impossible (cable ?)."));
    return;
  }

  // 2. Temps réellement écoulé depuis la dernière mesure.
  //    S'il y a eu un retard (affichage série...), on le limite au lieu
  //    de bloquer le robot.
  const uint32_t instant = micros();
  uint32_t ecoule = instant - dernier_echantillon_us;
  dernier_echantillon_us = instant;
  if (ecoule == 0) ecoule = 1;
  if (ecoule > DT_MAX_US) ecoule = DT_MAX_US;
  const float dt = ecoule * 1.0e-6f;

  // 3. Angle mesuré par l'accéléromètre
  float mesure;
  if (!mesurerAngle(mesure)) {
    changerEtat(ETAT_ATTENTE, F("Mesure d'angle invalide. Moteurs coupes."));
    afficherEtat();
    return;
  }
  derniere_mesure = mesure;

  // 4. Fusion gyro + accéléro avec le filtre de Kalman
  actualiserKalman(mesure, vitesseGyro(), dt);
  if (!isfinite(angle) || !isfinite(rate) || !isfinite(bias)) {
    changerEtat(ETAT_CALIBRATION, F("Filtre instable. Recalibration..."));
    return;
  }

  // 5. Erreur par rapport à la consigne
  const float erreur = normaliserAngle(consigne - angle);

  if (etat == ETAT_ATTENTE) {
    // Moteurs coupés tant que le robot n'est pas proche de l'équilibre.
    arreterMoteurs();
    if (fabsf(erreur) <= ANGLE_DEMARRAGE) {
      changerEtat(ETAT_EQUILIBRE, F("Regulation active."));
    }
  } else {
    // ÉTAT ÉQUILIBRE
    if (fabsf(erreur) > ANGLE_CHUTE) {
      changerEtat(ETAT_ATTENTE, F("Chute detectee. Moteurs coupes. Redresser le robot."));
    } else {
      // 6. PID puis commande des moteurs
      commande = calculerPID(erreur, dt);
      commanderMoteurs(commande);
    }
  }

  afficherEtat();
}