#include "PhiIMU.h"

/*
 ===========================================================
    PhiIMU.cpp — VERSION SCIENTIFIQUEMENT COHÉRENTE
    
    OBJECTIF SCIENTIFIQUE:
    Quantifier l'énergie des vibrations mécaniques ambiantes
    
    GRANDEUR MESURÉE:
    Proxy d'énergie vibratoire via magnitude RMS vectorielle
    de l'accélération dynamique (composante DC retirée)
    
    PRINCIPE PHYSIQUE:
    1. Accélération brute (BMI270) → vecteur 3D [ax, ay, az] en g
    2. Filtrage HPF (fc ≈ 1.7 Hz @ 90 Hz) → suppression gravité statique
    3. Magnitude vectorielle → ||a|| = √(ax² + ay² + az²)
    4. RMS temporel implicite via FFT → énergie fréquentielle
    
    UNITÉ EFFECTIVE:
    g RMS (root-mean-square acceleration)
    → Proxy linéaire de l'énergie cinétique vibratoire
    → E_vib ∝ m × a²_RMS (pour masse fixe du dispositif)
    
    CALIBRATION:
    • Baseline typique: 0.001 - 0.005 g (bruit thermique capteur)
    • Vibrations détectables: > 0.01 g (marche, transport)
    • Chocs mécaniques: > 0.5 g (chute, impact)
 ===========================================================
*/

PhiIMU::PhiIMU()
: _index(0),
  _ax_raw(0), _ay_raw(0), _az_raw(0),
  _mag_raw(0), _mag_filtered(0),
  _hpfX(0.88f), _hpfY(0.88f), _hpfZ(0.88f),
  _nextSample_us(0), _lastSample_us(0),
  _bufferReady(false)
{
    for (int i = 0; i < IMU_FIFO_SIZE; i++)
        _buffer[i] = 0.0f;
}

bool PhiIMU::begin() {
    if (!IMU.begin()) {
        Serial.println("✗ IMU init failed");
        return false;
    }

    uint32_t now = micros();
    _nextSample_us = now + IMU_TIMER_US;
    _lastSample_us = now;

    Serial.println("✓ PhiIMU (90 Hz vibration energy sampling)");
    return true;
}

void PhiIMU::update() {
    uint32_t now = micros();

    // ═══════════════════════════════════════════════════════════════════
    // ÉCHANTILLONNAGE TEMPOREL PRÉCIS (90 Hz)
    // ═══════════════════════════════════════════════════════════════════
    // fs = 90 Hz choisi pour:
    // • Nyquist > 45 Hz (vibrations mécaniques typiques < 40 Hz)
    // • Compatible FFT N=128 → résolution 0.7 Hz
    // • ODR BMI270 supportée (jusqu'à 1600 Hz)
    // ═══════════════════════════════════════════════════════════════════
    
    if ((int32_t)(now - _nextSample_us) < 0)
        return;

    if (!IMU.accelerationAvailable()) {
        _nextSample_us = now + IMU_TIMER_US;
        return;
    }

    _nextSample_us += IMU_TIMER_US;
    _lastSample_us = now;

    // ═══════════════════════════════════════════════════════════════════
    // ACQUISITION ACCÉLÉRATION BRUTE
    // ═══════════════════════════════════════════════════════════════════
    // BMI270 retourne accélération totale:
    // a_total = a_dynamique + g (gravité terrestre ≈ 1 g selon orientation)
    // ═══════════════════════════════════════════════════════════════════
    
    IMU.readAcceleration(_ax_raw, _ay_raw, _az_raw);

    // Magnitude brute (non utilisée pour FFT, diagnostic uniquement)
    _mag_raw = sqrtf(_ax_raw*_ax_raw + _ay_raw*_ay_raw + _az_raw*_az_raw);

    // ═══════════════════════════════════════════════════════════════════
    // FILTRAGE PASSE-HAUT (suppression composante statique)
    // ═══════════════════════════════════════════════════════════════════
    // HPF: y = (x - x₁) + α·y₁
    // Coefficient: α = 0.88
    // Fréquence de coupure: fc ≈ (1-α)·fs/(2π) = 0.12×90/6.28 ≈ 1.7 Hz
    // 
    // OBJECTIF:
    // Retirer gravité statique (1 g constant selon axe vertical)
    // → Isoler accélération dynamique (vibrations, mouvements)
    // 
    // JUSTIFICATION:
    // Gravité = signal DC en référentiel capteur
    // Vibrations = signal AC (variations temporelles > 1.7 Hz)
    // HPF élimine DC, conserve AC
    // 
    // ATTÉNUATION:
    // f < fc   : atténuation croissante (ex: 0.5 Hz → -6 dB)
    // f = fc   : -3 dB (50% amplitude)
    // f > 3·fc : transmission quasi-totale (>95%)
    // 
    // LIMITATION ASSUMÉE:
    // Mouvements lents (< 1.7 Hz) partiellement atténués
    // → Acceptable: vibrations mécaniques typiques > 2 Hz
    // ═══════════════════════════════════════════════════════════════════
    
    float fx = _hpfX.step(_ax_raw);
    float fy = _hpfY.step(_ay_raw);
    float fz = _hpfZ.step(_az_raw);

    // ═══════════════════════════════════════════════════════════════════
    // MAGNITUDE VECTORIELLE FILTRÉE
    // ═══════════════════════════════════════════════════════════════════
    // ||a_dyn|| = √(fx² + fy² + fz²)
    // 
    // INTERPRÉTATION PHYSIQUE:
    // • Magnitude = norme euclidienne du vecteur accélération dynamique
    // • Insensible à l'orientation du capteur (invariant rotationnel)
    // • Proportionnelle à √(E_vib) pour vibrations isotropes
    // 
    // UNITÉ:
    // g RMS (root-mean-square) après analyse FFT
    // 1 g = 9.81 m/s² (accélération gravitationnelle standard)
    // ═══════════════════════════════════════════════════════════════════
    
    _mag_filtered = sqrtf(fx*fx + fy*fy + fz*fz);

    // ═══════════════════════════════════════════════════════════════════
    // STOCKAGE FIFO POUR FFT
    // ═══════════════════════════════════════════════════════════════════
    // Buffer circulaire N=128 échantillons
    // → Fenêtre temporelle: 128/90 ≈ 1.42 secondes
    // → Résolution fréquentielle: 90/128 ≈ 0.7 Hz
    // ═══════════════════════════════════════════════════════════════════
    
    _buffer[_index++] = _mag_filtered;

    if (_index >= IMU_FIFO_SIZE) {
        _index = 0;
        _bufferReady = true;
    }
}

bool PhiIMU::readyForFFT() const {
    return _bufferReady;
}

const float* PhiIMU::getFFTBuffer() const {
    return _buffer;
}