#include "IMU.h"
#include <ArduinoJson.h>
#include <LittleFS.h>

void calibrateMagn();
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
float invSqrt(float x);
bool imuSaveMagnCalibration();

/******************************************************************************
 * IMU module                                                                 *
 ******************************************************************************/
// #define S_SCL   33
// #define S_SDA   32

AK09918_err_type_t err;

QMI8658 qmi8658_;
AK09918 magnetometer_;

int16_t offset_x = -12, offset_y = 0, offset_z = 0;
int16_t x, y, z;
float magnetic_declination_deg = 0.0f;
float heading_offset_deg = 0.0f;   // manual offset to align sensor X axis with rover forward

// Last calibration result for web feedback: -1=none, 0=failed, 1=success
static int8_t g_lastCalStatus = -1;

#define Kp 4.50f   // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 1.0f    // integral gain governs rate of convergence of gyroscope biases

float angles[3];
float q0, q1, q2, q3; 
float exInt = 0.0f;
float eyInt = 0.0f;
float ezInt = 0.0f;
uint32_t lastFilterUpdateUs = 0;

namespace {
constexpr float kDegToRad = 0.01745329251994329577f;
constexpr float kRadToDeg = 57.295779513082320876f;
constexpr float kDefaultSampleDt = 0.01f;
constexpr float kMaxSampleDt = 0.1f;
constexpr float kMagFilterAlpha = 0.2f;
constexpr float kHeadingFilterAlpha = 0.25f;
constexpr int16_t kMinHeadingCalibrationSpan = 30;
constexpr int16_t kMinZCalibrationSpan = 15;
constexpr uint32_t kDefaultMagCalibrationDurationMs = 12000;
constexpr char kImuCalibrationFile[] = "/imuConfig.json";

struct MagCalibrationState {
  int16_t minX;
  int16_t maxX;
  int16_t minY;
  int16_t maxY;
  int16_t minZ;
  int16_t maxZ;
  bool initialized;
};

MagCalibrationState magCalibrationSession = {0, 0, 0, 0, 0, 0, false};
bool magCalibrationAvailable = false;
bool magCalibrationStored = false;
bool magCalibrationRunning = false;
uint32_t magCalibrationStartedMs = 0;
uint32_t magCalibrationDurationMs = kDefaultMagCalibrationDurationMs;
uint8_t magCalibrationProgress = 0;
bool filteredMagInitialized = false;
float filteredMagX = 0.0f;
float filteredMagY = 0.0f;
float filteredMagZ = 0.0f;
bool filteredHeadingInitialized = false;
float filteredHeadingDeg = 0.0f;

float clampUnit(float value)
{
  if (value > 1.0f) {
    return 1.0f;
  }
  if (value < -1.0f) {
    return -1.0f;
  }
  return value;
}

void resetFilterState()
{
  q0 = 1.0f;
  q1 = 0.0f;
  q2 = 0.0f;
  q3 = 0.0f;
  exInt = 0.0f;
  eyInt = 0.0f;
  ezInt = 0.0f;
  lastFilterUpdateUs = micros();
}

void resetHeadingState()
{
  filteredMagInitialized = false;
  filteredMagX = 0.0f;
  filteredMagY = 0.0f;
  filteredMagZ = 0.0f;
  filteredHeadingInitialized = false;
  filteredHeadingDeg = 0.0f;
}

void resetMagCalibrationState()
{
  magCalibrationSession = {0, 0, 0, 0, 0, 0, false};
  magCalibrationProgress = 0;
}

float wrapDegrees360(float angleDeg)
{
  while (angleDeg < 0.0f) {
    angleDeg += 360.0f;
  }
  while (angleDeg >= 360.0f) {
    angleDeg -= 360.0f;
  }
  return angleDeg;
}

float wrapDegrees180(float angleDeg)
{
  float wrapped = wrapDegrees360(angleDeg);
  if (wrapped > 180.0f) {
    wrapped -= 360.0f;
  }
  return wrapped;
}

float lowPassHeading(float previousDeg, float currentDeg, float alpha)
{
  const float deltaDeg = wrapDegrees180(currentDeg - previousDeg);
  return wrapDegrees360(previousDeg + alpha * deltaDeg);
}

float getSampleDeltaSeconds()
{
  const uint32_t nowUs = micros();

  if (lastFilterUpdateUs == 0) {
    lastFilterUpdateUs = nowUs;
    return kDefaultSampleDt;
  }

  const uint32_t elapsedUs = nowUs - lastFilterUpdateUs;
  lastFilterUpdateUs = nowUs;

  const float deltaSeconds = elapsedUs * 1.0e-6f;
  if (deltaSeconds <= 0.0f || deltaSeconds > kMaxSampleDt) {
    return kDefaultSampleDt;
  }

  return deltaSeconds;
}

bool normalizeVector(float *xAxis, float *yAxis, float *zAxis)
{
  const float normSquared = (*xAxis * *xAxis) + (*yAxis * *yAxis) + (*zAxis * *zAxis);
  if (normSquared <= 1.0e-12f) {
    return false;
  }

  const float norm = invSqrt(normSquared);
  *xAxis *= norm;
  *yAxis *= norm;
  *zAxis *= norm;
  return true;
}

bool hasEnoughMagCalibrationCoverage(const MagCalibrationState &state)
{
  if (!state.initialized) {
    return false;
  }

  const int16_t spanX = state.maxX - state.minX;
  const int16_t spanY = state.maxY - state.minY;
  return spanX >= kMinHeadingCalibrationSpan && spanY >= kMinHeadingCalibrationSpan;
}

void updateMagCalibrationSession(int16_t rawX, int16_t rawY, int16_t rawZ)
{
  if (!magCalibrationRunning) {
    return;
  }

  if (!magCalibrationSession.initialized) {
    magCalibrationSession.minX = rawX;
    magCalibrationSession.maxX = rawX;
    magCalibrationSession.minY = rawY;
    magCalibrationSession.maxY = rawY;
    magCalibrationSession.minZ = rawZ;
    magCalibrationSession.maxZ = rawZ;
    magCalibrationSession.initialized = true;
  } else {
    if (rawX < magCalibrationSession.minX) magCalibrationSession.minX = rawX;
    if (rawX > magCalibrationSession.maxX) magCalibrationSession.maxX = rawX;
    if (rawY < magCalibrationSession.minY) magCalibrationSession.minY = rawY;
    if (rawY > magCalibrationSession.maxY) magCalibrationSession.maxY = rawY;
    if (rawZ < magCalibrationSession.minZ) magCalibrationSession.minZ = rawZ;
    if (rawZ > magCalibrationSession.maxZ) magCalibrationSession.maxZ = rawZ;
  }

  const uint32_t elapsedMs = millis() - magCalibrationStartedMs;
  if (magCalibrationDurationMs == 0) {
    magCalibrationProgress = 100;
  } else {
    const uint32_t clampedProgress = (elapsedMs >= magCalibrationDurationMs)
      ? 100
      : (elapsedMs * 100UL) / magCalibrationDurationMs;
    magCalibrationProgress = (uint8_t)clampedProgress;
  }

  if (elapsedMs < magCalibrationDurationMs) {
    return;
  }

  magCalibrationRunning = false;
  magCalibrationProgress = 100;

  if (!hasEnoughMagCalibrationCoverage(magCalibrationSession)) {
    g_lastCalStatus = 0;
    String resultJson = String("{\"T\":1002,\"status\":0,\"info\":\"Mag calibration failed. Rotate slower and cover more angles.\",\"magCal\":") +
                        (magCalibrationAvailable ? "1" : "0") +
                        ",\"saved\":" + (magCalibrationStored ? "1" : "0") + "}";
    Serial.println(resultJson);
    return;
  }

  const int16_t spanZ = magCalibrationSession.maxZ - magCalibrationSession.minZ;
  offset_x = (magCalibrationSession.maxX + magCalibrationSession.minX) / 2;
  offset_y = (magCalibrationSession.maxY + magCalibrationSession.minY) / 2;
  if (spanZ >= kMinZCalibrationSpan) {
    offset_z = (magCalibrationSession.maxZ + magCalibrationSession.minZ) / 2;
  }

  magCalibrationAvailable = true;
  resetHeadingState();
  resetFilterState();
  magCalibrationStored = imuSaveMagnCalibration();
  g_lastCalStatus = 1;

  String resultJson = String("{\"T\":1002,\"status\":1,\"info\":\"Mag calibration finished.\",\"magCal\":1,\"saved\":") +
                      (magCalibrationStored ? "1" : "0") +
                      ",\"x\":" + String(offset_x) +
                      ",\"y\":" + String(offset_y) +
                      ",\"z\":" + String(offset_z) + "}";
  Serial.println(resultJson);
}

bool headingCalibrationReady()
{
  return magCalibrationAvailable;
}

void lowPassMagneticSample(float rawX, float rawY, float rawZ,
                           float *correctedX, float *correctedY, float *correctedZ)
{
  if (!filteredMagInitialized) {
    filteredMagX = rawX;
    filteredMagY = rawY;
    filteredMagZ = rawZ;
    filteredMagInitialized = true;
  } else {
    filteredMagX += (rawX - filteredMagX) * kMagFilterAlpha;
    filteredMagY += (rawY - filteredMagY) * kMagFilterAlpha;
    filteredMagZ += (rawZ - filteredMagZ) * kMagFilterAlpha;
  }

  if (correctedX != nullptr) {
    *correctedX = filteredMagX - offset_x;
  }
  if (correctedY != nullptr) {
    *correctedY = filteredMagY - offset_y;
  }
  if (correctedZ != nullptr) {
    *correctedZ = filteredMagZ - offset_z;
  }
}

bool computeTiltCompensatedHeading(float rollDeg, float pitchDeg, float mx, float my, float mz, float *headingDeg)
{
  if (headingDeg == nullptr) {
    return false;
  }

  const float rollRad = rollDeg * kDegToRad;
  const float pitchRad = pitchDeg * kDegToRad;

  const float xHeading = mx * cosf(pitchRad) + mz * sinf(pitchRad);
  const float yHeading = mx * sinf(rollRad) * sinf(pitchRad) +
                         my * cosf(rollRad) -
                         mz * sinf(rollRad) * cosf(pitchRad);

  if (fabsf(xHeading) <= 1.0e-6f && fabsf(yHeading) <= 1.0e-6f) {
    return false;
  }

  // Negative yHeading: atan2(y,x) is CCW but compass convention is CW from North.
  // See Freescale AN4248 eq.22: heading = atan2(-Bfy, Bfx).
  *headingDeg = wrapDegrees360(atan2f(-yHeading, xHeading) * kRadToDeg + magnetic_declination_deg + heading_offset_deg);
  return true;
}
}  // namespace

void imuInit()
{
    // Wire.begin(S_SDA, S_SCL);
    // Serial.begin(115200);
   
    if (qmi8658_.begin() == 0)
	      Serial.println("qmi8658_init fail");

    // Verify AK09918 is present on I2C.
    // WIA1=0x48 (AKM company ID), WIA2=0x0C (AK09918 device ID).
    {
        const uint16_t devId = magnetometer_.getDeviceID();
        const uint8_t wia1 = (uint8_t)(devId >> 8);
        const uint8_t wia2 = (uint8_t)(devId & 0xFF);
        if (wia1 != 0x48 || wia2 != 0x0C) {
            Serial.printf("AK09918 warning: WIA1=0x%02X WIA2=0x%02X (expected 0x48/0x0C). "
                          "Check I2C on SDA=GPIO32 SCL=GPIO33, address 0x0C.\n", wia1, wia2);
        } else {
            Serial.println("AK09918 found (WIA OK).");
        }
    }

    // Datasheet §6: must transition through power-down before setting any
    // measurement mode.  No DRDY polling needed here — the main loop reads
    // data continuously; we just need to get the mode register set.
    magnetometer_.initialize(AK09918_POWER_DOWN);
    delay(10);
    magnetometer_.switchMode(AK09918_CONTINUOUS_100HZ);
    // Wait 5 full sample periods (100 Hz → 10 ms each) so first data is ready.
    delay(60);

    // Diagnostic: read ST1 and raw data to confirm the sensor is alive.
    {
        int16_t tx = 0, ty = 0, tz = 0;
        const AK09918_err_type_t tErr = magnetometer_.getData(&tx, &ty, &tz);
        const AK09918_err_type_t rdyErr = magnetometer_.isDataReady();
        Serial.printf("AK09918 diag: getData err=%d x=%d y=%d z=%d  ST1_ready=%d\n",
                      tErr, tx, ty, tz, rdyErr == AK09918_ERR_OK ? 1 : 0);
        if (tErr == AK09918_ERR_OK || tErr == AK09918_ERR_OVERFLOW) {
            Serial.println("AK09918 producing data.");
        } else {
            Serial.printf("AK09918 getData failed (err=%d). "
                          "Sensor may be in power-down - check 3V3 supply.\n", tErr);
        }
    }
    // Serial.println("Start figure-8 calibration after 1 seconds.");
    // delay(1000);
    // calibrate(10000, &offset_x, &offset_y, &offset_z);
    // calibrateMagn();
    resetMagCalibrationState();
    resetHeadingState();
    resetFilterState();
}

void imuDataGet(EulerAngles *pstAngles, 
                IMU_ST_SENSOR_DATA_FLOAT *pstGyroRawData,
                IMU_ST_SENSOR_DATA_FLOAT *pstAccelRawData,
                IMU_ST_SENSOR_DATA *pstMagnRawData)
{
  if (pstAngles == nullptr || pstGyroRawData == nullptr ||
      pstAccelRawData == nullptr || pstMagnRawData == nullptr) {
    return;
  }

  float  acc[3] = {0.0f, 0.0f, 0.0f};
  float  gyro[3] = {0.0f, 0.0f, 0.0f};
  float MotionVal[9];
  float correctedMagX = 0.0f;
  float correctedMagY = 0.0f;
  float correctedMagZ = 0.0f;

  const AK09918_err_type_t magErr = magnetometer_.getData(&x, &y, &z);
  if (magErr == AK09918_ERR_OVERFLOW) {
    Serial.println("AK09918 overflow detected, keeping last valid magnetic sample.");
  }
  if (magErr == AK09918_ERR_OK || magErr == AK09918_ERR_OVERFLOW) {
    updateMagCalibrationSession(x, y, z);
    // Only feed the low-pass filter when we have real data.
    lowPassMagneticSample((float)x, (float)y, (float)z, &correctedMagX, &correctedMagY, &correctedMagZ);
  } else {
    // Read failed: reuse last corrected values (filter state stays unchanged).
    correctedMagX = filteredMagX - offset_x;
    correctedMagY = filteredMagY - offset_y;
    correctedMagZ = filteredMagZ - offset_z;
  }

  pstMagnRawData->s16X = (int16_t)lroundf(correctedMagX);
  pstMagnRawData->s16Y = (int16_t)lroundf(correctedMagY);
  pstMagnRawData->s16Z = (int16_t)lroundf(correctedMagZ);

  // qmi8658_.GetEulerAngles(&pstAngles->pitch,&pstAngles->roll,&pstAngles->yaw,acc,gyro);
  qmi8658_.read_sensor_data(acc,gyro);

  // pstAngles->roll = atan2((float)acc[1], (float)acc[2]);
  // pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2])));

  // double Xheading = pstMagnRawData->s16X * cos(pstAngles->pitch) + pstMagnRawData->s16Y * sin(pstAngles->roll) * sin(pstAngles->pitch) + pstMagnRawData->s16Z * cos(pstAngles->roll) * sin(pstAngles->pitch);
  // double Yheading = pstMagnRawData->s16Y * cos(pstAngles->roll) - pstMagnRawData->s16Z * sin(pstAngles->pitch);
  
  // pstAngles->yaw = 57.3 * atan2(Yheading, Xheading) + magnetic_declination_deg;

  // pstAngles->roll = atan2((float)acc[1], (float)acc[2]) * 57.3;
  // pstAngles->pitch = atan2(-(float)acc[0], sqrt((float)(acc[1] * acc[1]) + (float)(acc[2] * acc[2]))) * 57.3;
  MotionVal[0]=gyro[0];
  MotionVal[1]=gyro[1];
  MotionVal[2]=gyro[2];
  MotionVal[3]=acc[0];
  MotionVal[4]=acc[1];
  MotionVal[5]=acc[2];
  MotionVal[6]=pstMagnRawData->s16X;
  MotionVal[7]=pstMagnRawData->s16Y;
  MotionVal[8]=pstMagnRawData->s16Z;

  const bool useMagneticHeading = headingCalibrationReady();
  imuAHRSupdate((float)MotionVal[0] * kDegToRad, (float)MotionVal[1] * kDegToRad, (float)MotionVal[2] * kDegToRad,
                (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], 
                useMagneticHeading ? (float)MotionVal[6] : 0.0f,
                useMagneticHeading ? (float)MotionVal[7] : 0.0f,
                useMagneticHeading ? MotionVal[8] : 0.0f);

  pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f;
  pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1,
                           -2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f;
  // Standard ZYX Euler: yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2^2 + q3^2))
  // Previous formula had both signs inverted → always started at 180° instead of 0°.
  const float quaternionYawDeg = wrapDegrees360(
      atan2f(2.0f * q1 * q2 + 2.0f * q0 * q3,
             1.0f - 2.0f * q2 * q2 - 2.0f * q3 * q3) * kRadToDeg);

  float headingDeg = 0.0f;
  if (useMagneticHeading &&
      computeTiltCompensatedHeading(pstAngles->roll, pstAngles->pitch,
                                    correctedMagX, correctedMagY, correctedMagZ,
                                    &headingDeg)) {
    if (!filteredHeadingInitialized) {
      filteredHeadingDeg = headingDeg;
      filteredHeadingInitialized = true;
    } else {
      filteredHeadingDeg = lowPassHeading(filteredHeadingDeg, headingDeg, kHeadingFilterAlpha);
    }
    pstAngles->yaw = filteredHeadingDeg;
  } else {
    pstAngles->yaw = quaternionYawDeg;
  }

  pstGyroRawData->X = gyro[0];
  pstGyroRawData->Y = gyro[1];
  pstGyroRawData->Z = gyro[2];

  pstAccelRawData->X = acc[0];
  pstAccelRawData->Y = acc[1];
  pstAccelRawData->Z = acc[2];

  return;  
}

bool imuRecalibrate()
{
  qmi8658_.autoOffsets();
  resetHeadingState();
  resetFilterState();
  return true;
}

void imuGetMagnOffsets(IMU_ST_SENSOR_DATA *pstMagnOffset)
{
  if (pstMagnOffset == nullptr) {
    return;
  }

  pstMagnOffset->s16X = offset_x;
  pstMagnOffset->s16Y = offset_y;
  pstMagnOffset->s16Z = offset_z;
}

void imuSetMagnOffsets(int16_t inputX, int16_t inputY, int16_t inputZ)
{
  offset_x = inputX;
  offset_y = inputY;
  offset_z = inputZ;
  magCalibrationAvailable = true;
  resetHeadingState();
  resetFilterState();
}

bool imuHasHeadingCalibration()
{
  return headingCalibrationReady();
}

bool imuHasStoredMagnCalibration()
{
  return magCalibrationStored;
}

bool imuIsMagnCalibrationRunning()
{
  return magCalibrationRunning;
}

uint8_t imuGetMagnCalibrationProgress()
{
  return magCalibrationProgress;
}

bool imuStartMagnCalibration(uint32_t durationMs)
{
  resetMagCalibrationState();
  resetHeadingState();
  magCalibrationRunning = true;
  magCalibrationStartedMs = millis();
  magCalibrationDurationMs = durationMs < 3000 ? kDefaultMagCalibrationDurationMs : durationMs;
  magCalibrationProgress = 0;
  return true;
}

bool imuLoadMagnCalibration()
{
  if (!LittleFS.exists(kImuCalibrationFile)) {
    magCalibrationAvailable = false;
    magCalibrationStored = false;
    return false;
  }

  File configFile = LittleFS.open(kImuCalibrationFile, "r");
  if (!configFile) {
    magCalibrationAvailable = false;
    magCalibrationStored = false;
    return false;
  }

  StaticJsonDocument<128> imuDoc;
  const DeserializationError err = deserializeJson(imuDoc, configFile);
  configFile.close();
  if (err) {
    magCalibrationAvailable = false;
    magCalibrationStored = false;
    return false;
  }

  if (!imuDoc.containsKey("offset_x") ||
      !imuDoc.containsKey("offset_y") ||
      !imuDoc.containsKey("offset_z")) {
    magCalibrationAvailable = false;
    magCalibrationStored = false;
    return false;
  }

  offset_x = imuDoc["offset_x"].as<int16_t>();
  offset_y = imuDoc["offset_y"].as<int16_t>();
  offset_z = imuDoc["offset_z"].as<int16_t>();
  // Backwards compatible: version 1 files won't have these keys, defaults to 0.
  magnetic_declination_deg = imuDoc["decl"] | 0.0f;
  heading_offset_deg        = imuDoc["hOff"] | 0.0f;

  magCalibrationAvailable = true;
  magCalibrationStored = true;
  resetHeadingState();
  resetFilterState();
  return true;
}

bool imuSaveMagnCalibration()
{
  File configFile = LittleFS.open(kImuCalibrationFile, "w");
  if (!configFile) {
    return false;
  }

  StaticJsonDocument<192> imuDoc;
  imuDoc["offset_x"] = offset_x;
  imuDoc["offset_y"] = offset_y;
  imuDoc["offset_z"] = offset_z;
  imuDoc["decl"]     = magnetic_declination_deg;
  imuDoc["hOff"]     = heading_offset_deg;
  imuDoc["version"]  = 2;

  const size_t bytesWritten = serializeJson(imuDoc, configFile);
  configFile.println();
  configFile.close();

  magCalibrationStored = bytesWritten > 0;
  return magCalibrationStored;
}

float imuGetTemperature()
{
  return qmi8658_.read_temperature();
}

void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
{
  const float sampleDt = getSampleDeltaSeconds();
  const float halfT = 0.5f * sampleDt;
  float hx = 0.0f, hy = 0.0f, hz = 0.0f, bx = 0.0f, bz = 0.0f;
  float vx = 0.0f, vy = 0.0f, vz = 0.0f, wx = 0.0f, wy = 0.0f, wz = 0.0f;
  float ex = 0.0f, ey = 0.0f, ez = 0.0f;

  float q0q0 = q0 * q0;
  float q0q1 = q0 * q1;
  float q0q2 = q0 * q2;
  float q0q3 = q0 * q3;
  float q1q1 = q1 * q1;
  float q1q2 = q1 * q2;
  float q1q3 = q1 * q3;
  float q2q2 = q2 * q2;   
  float q2q3 = q2 * q3;
  float q3q3 = q3 * q3;          

  const bool accelValid = normalizeVector(&ax, &ay, &az);
  const bool magnValid = normalizeVector(&mx, &my, &mz);

  if (accelValid) {
    vx = 2.0f * (q1q3 - q0q2);
    vy = 2.0f * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    ex += (ay * vz - az * vy);
    ey += (az * vx - ax * vz);
    ez += (ax * vy - ay * vx);
  }

  if (magnValid) {
    hx = 2.0f * mx * (0.5f - q2q2 - q3q3) + 2.0f * my * (q1q2 - q0q3) + 2.0f * mz * (q1q3 + q0q2);
    hy = 2.0f * mx * (q1q2 + q0q3) + 2.0f * my * (0.5f - q1q1 - q3q3) + 2.0f * mz * (q2q3 - q0q1);
    hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
    bx = sqrtf((hx * hx) + (hy * hy));
    bz = hz;

    wx = 2.0f * bx * (0.5f - q2q2 - q3q3) + 2.0f * bz * (q1q3 - q0q2);
    wy = 2.0f * bx * (q1q2 - q0q3) + 2.0f * bz * (q0q1 + q2q3);
    wz = 2.0f * bx * (q0q2 + q1q3) + 2.0f * bz * (0.5f - q1q1 - q2q2);

    ex += (my * wz - mz * wy);
    ey += (mz * wx - mx * wz);
    ez += (mx * wy - my * wx);
  }

  if (accelValid || magnValid) {
    exInt += ex * Ki * sampleDt;
    eyInt += ey * Ki * sampleDt;
    ezInt += ez * Ki * sampleDt;

    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;
  }

  const float q0Prev = q0;
  const float q1Prev = q1;
  const float q2Prev = q2;
  const float q3Prev = q3;

  q0 = q0Prev + (-q1Prev * gx - q2Prev * gy - q3Prev * gz) * halfT;
  q1 = q1Prev + (q0Prev * gx + q2Prev * gz - q3Prev * gy) * halfT;
  q2 = q2Prev + (q0Prev * gy - q1Prev * gz + q3Prev * gx) * halfT;
  q3 = q3Prev + (q0Prev * gz + q1Prev * gy - q2Prev * gx) * halfT;

  const float normSquared = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
  if (normSquared <= 1.0e-12f) {
    resetFilterState();
    return;
  }

  const float norm = invSqrt(normSquared);
  q0 = q0 * norm;
  q1 = q1 * norm;
  q2 = q2 * norm;
  q3 = q3 * norm;
}

float invSqrt(float x) 
{
  if (x <= 0.0f) {
    return 0.0f;
  }

  float halfx = 0.5f * x;
  union {
    float f;
    uint32_t i;
  } conv = { x };

  conv.i = 0x5f3759df - (conv.i >> 1);

  float y = conv.f;
  y = y * (1.5f - (halfx * y * y));

  return y;
}

void imuSetMagneticDeclination(float deg)
{
  magnetic_declination_deg = deg;
}

float imuGetMagneticDeclination()
{
  return magnetic_declination_deg;
}

void imuSetHeadingOffset(float deg)
{
  heading_offset_deg = deg;
}

float imuGetHeadingOffset()
{
  return heading_offset_deg;
}

// Returns -1 if no result yet, 0 if last cal failed, 1 if last cal succeeded.
// Clears the result after reading so it is reported only once.
int8_t imuPopLastCalStatus()
{
  const int8_t s = g_lastCalStatus;
  g_lastCalStatus = -1;
  return s;
}

void calibrateMagn(void)
{
  int16_t temp[9];
  Serial.printf("keep 10dof-imu device horizontal and it will read x y z axis offset value after 4 seconds\n");
  delay(4000);
  Serial.printf("start read all axises offset value\n");
  magnetometer_.getData(&x, &y, &z);
  temp[0] = x;
  temp[1] = y;
  temp[2] = z;
  
  Serial.printf("rotate z axis 180 degrees and it will read all axises offset value after 4 seconds\n");
  delay(4000);
  Serial.printf("start read all axises offset value\n");
  magnetometer_.getData(&x, &y, &z);
  temp[3] = x;
  temp[4] = y;
  temp[5] = z;

  Serial.printf("flip 10dof-imu device and keep it horizontal and it will read all axises offset value after 4 seconds\n");
  delay(4000);
  Serial.printf("start read all axises offset value\n");
  magnetometer_.getData(&x, &y, &z);
  temp[6] = x;
  temp[7] = y;
  temp[8] = z;

  offset_x = (temp[0]+temp[3])/2;
  offset_y = (temp[1]+temp[4])/2;
  offset_z = (temp[5]+temp[8])/2;
}