#include "PhiMagnetometer.h"

/*
 ===========================================================
    PhiMagnetometer.cpp — Détection champ magnétique
 ===========================================================
*/

PhiMagnetometer::PhiMagnetometer()
: _x(0), _y(0), _z(0),
  _magnitude(0),
  _deltaMag(0),
  _baseline(0),
  _calibrated(false),
  _lpf(0.4f)
{}

bool PhiMagnetometer::begin() {
    // IMU.begin() doit avoir été appelé avant
    // BMM150 est auto-initialisé avec le BMI270

    Serial.println("✔ PhiMagnetometer initialisé (BMM150)");
    return true;
}

void PhiMagnetometer::calibrateBaseline(uint16_t samples) {
    Serial.print("🧲 Calibration baseline magnétique (");
    Serial.print(samples);
    Serial.println(" échantillons)...");

    float sum = 0;
    uint16_t validSamples = 0;

    for (uint16_t i = 0; i < samples; i++) {
        if (IMU.magneticFieldAvailable()) {
            float x, y, z;
            IMU.readMagneticField(x, y, z);
            sum += sqrtf(x*x + y*y + z*z);
            validSamples++;
        }
        delay(10);  // 100 Hz max rate
    }

    if (validSamples > 0) {
        _baseline = sum / validSamples;
        _calibrated = true;

        Serial.print("   Baseline: ");
        Serial.print(_baseline, 2);
        Serial.println(" µT");
        Serial.println("   ✅ Calibration OK");
    } else {
        Serial.println("   ❌ Calibration échec (BMM150 non disponible)");
    }
}

void PhiMagnetometer::update() {
    if (!IMU.magneticFieldAvailable())
        return;

    // Lecture composantes
    IMU.readMagneticField(_x, _y, _z);

    // Magnitude totale
    float mag = sqrtf(_x*_x + _y*_y + _z*_z);

    // Lissage exponentiel
    _magnitude = _lpf.step(mag);

    // Delta vs baseline (perturbations)
    if (_calibrated) {
        _deltaMag = fabsf(_magnitude - _baseline);
    } else {
        _deltaMag = 0;
    }
}