// ---------------------------------------------------------------------------
// baro_sync.h
// Neil Bowen - AltimeterCloud.com
//
// Barometer group delay, computed from configuration instead of detected at
// launch.
//
//   t_sync = t_conv/2 + T_odr/2 + c_iir * T_odr + t_kal(v) - t_imu
//
// Call it once, after sensor detection and settings load. The result is the
// number of milliseconds by which a logged pressure altitude sits behind the
// truth, so it is also the amount by which the IMU columns should be delayed
// to line the two streams up.
//
// Assumes the firmware polls the sensor (rather than reading it on its data
// ready interrupt) and passes the altitude through a SimpleKalmanFilter at the
// loop rate. If your pipeline differs, the first three terms still hold and
// the Kalman term is whatever your own filter contributes.
// ---------------------------------------------------------------------------

#ifndef BARO_SYNC_H
#define BARO_SYNC_H

#include <math.h>

struct BaroSyncCfg {
  float t_conv_ms;    // sensor conversion time at the configured oversampling
  float odr_hz;       // sensor output data rate in flight (NOT the loop rate)
  float iir_coeff;    // Bosch IIR coefficient: 0, 1, 3, 7, 15, 31 (0 = bypass)
  float e_mea;        // Kalman measurement uncertainty
  float q_base;       // Kalman process noise, expressed at 50 Hz
  float loop_hz;      // firmware sample rate
  float imu_odr_hz;   // IMU output data rate in flight
  float v_ref_ms;     // reference airspeed for the Kalman term, m/s
};

// Steady-state lag of a SimpleKalmanFilter tracking a constant-rate ramp.
// The gain settles where K^2 * e_mea = q * s * (1 - K), with s the true climb
// per loop, and the estimate then trails the measurement by (1 - K)/K loops.
static inline float kalmanLagMs(float e_mea, float q, float v_ms, float loop_hz) {
  if (q <= 0.0f || e_mea <= 0.0f || v_ms <= 0.0f || loop_hz <= 0.0f) return 0.0f;
  const float T_loop_ms = 1000.0f / loop_hz;
  const float s  = v_ms / loop_hz;
  const float qs = q * s;
  const float K  = (-qs + sqrtf(qs * qs + 4.0f * e_mea * qs)) / (2.0f * e_mea);
  if (K <= 0.0f || K >= 1.0f) return 0.0f;
  return (1.0f - K) / K * T_loop_ms;
}

static inline float baroSyncMs(const BaroSyncCfg &c) {
  const float T_odr_ms = (c.odr_hz > 0.0f) ? 1000.0f / c.odr_hz : 0.0f;
  const float q        = c.q_base * (50.0f / c.loop_hz);  // process noise scaled with dt

  const float t_conv = c.t_conv_ms * 0.5f;                // value is the mid-conversion instant
  const float t_poll = T_odr_ms * 0.5f;                   // mean age of a polled sample
  const float t_iir  = c.iir_coeff * T_odr_ms;            // c samples at the SENSOR rate
  const float t_kal  = kalmanLagMs(c.e_mea, q, c.v_ref_ms, c.loop_hz);
  const float t_imu  = (c.imu_odr_hz > 0.0f) ? 1500.0f / c.imu_odr_hz : 0.0f;

  return t_conv + t_poll + t_iir + t_kal - t_imu;
}

static inline int baroSyncSamples(const BaroSyncCfg &c) {
  return (int)lroundf(baroSyncMs(c) * c.loop_hz / 1000.0f);
}

// Conversion time in ms, Bosch BMP388 and BMP390 datasheet formula.
// Pass the oversampling factors themselves, so x8 pressure and x1 temperature
// is bmp390ConvMs(8, 1). Note that a library call named OVERSAMPLING_SKIP may
// still be x1 rather than genuinely off, so check before passing 0.
static inline float bmp390ConvMs(int osr_p, int osr_t) {
  return (234.0f + (392.0f + osr_p * 2020.0f) + (163.0f + osr_t * 2020.0f)) / 1000.0f;
}

// BMP581 conversion time at pressure OSR x16 with temperature at x1, taken
// from the highest ODR the part will accept at that oversampling (about
// 87.6 Hz). This is the one number here that is inferred rather than read
// from a datasheet formula. If you run a different oversampling, measure it
// or look it up rather than scaling this.
#define BMP581_CONV_MS_OSR16  11.4f

// ---------------------------------------------------------------------------
// BMP280 and BME280
//
// Two things differ from the BMP388/BMP390 family and both bite.
//
// 1. The IIR coefficient convention is different. The BMP280 datasheet defines
//    the filter as (old * (coef - 1) + adc) / coef with coef of 2, 4, 8 or 16,
//    so a "filter 16" setting is a 15 sample pole, not a 16 sample one. Use
//    bmp280IirCoeff() to convert. (Cross-check: the datasheet's "samples to
//    reach 75% of step response" column gives 2, 5, 11, 22, which is what a
//    15 sample pole produces at coef 16.)
//
// 2. In normal mode you do not choose an ODR directly. It falls out of the
//    oversampling and the standby time, and the IIR runs at that rate. A long
//    standby therefore multiplies straight into the IIR term.
// ---------------------------------------------------------------------------

// Typical measurement time in ms, BMP280 datasheet Table 13.
// Pass oversampling factors, or 0 for a skipped channel.
static inline float bmp280MeasMs(int osr_p, int osr_t) {
  float t = 1.0f;
  if (osr_t > 0) t += 2.0f * osr_t;
  if (osr_p > 0) t += 2.0f * osr_p + 0.5f;
  return t;   // worst case is roughly 15% longer, see the datasheet max column
}

// Normal-mode output data rate: one measurement plus one standby period.
static inline float bmp280OdrHz(int osr_p, int osr_t, float standby_ms) {
  return 1000.0f / (bmp280MeasMs(osr_p, osr_t) + standby_ms);
}

// Convert a BMP280 filter setting (2, 4, 8, 16) to the pole count this
// equation wants. Anything below 2 is the filter switched off.
static inline float bmp280IirCoeff(int filter_setting) {
  return (filter_setting >= 2) ? (float)(filter_setting - 1) : 0.0f;
}

#endif  // BARO_SYNC_H