/*
 * baro.c — BME280/BMP280 barometric pressure & ambient temperature module
 *          (Issue #672).
 *
 * Reads pressure, temperature, and (on BME280) humidity from the sensor at
 * BARO_READ_HZ (1 Hz). Computes pressure altitude using bmp280_pressure_to_alt_cm()
 * (ISA barometric formula, p0 = 101325 Pa). Publishes JLINK_TLM_BARO (0x8D)
 * telemetry to the Orin at BARO_TLM_HZ (1 Hz).
 *
 * Runs entirely on the Mamba F722S. No Orin dependency.
 * baro_get_alt_cm() exposes altitude for slope compensation in the balance PID.
 */

#include "baro.h"
#include "bmp280.h"
#include "jlink.h"

static int         s_chip_id     = 0;   /* 0x58=BMP280, 0x60=BME280, 0=absent */
static baro_data_t s_data;              /* latest reading                       */
static uint32_t    s_last_read_ms;      /* timestamp of last I2C read           */
static uint32_t    s_last_tlm_ms;       /* timestamp of last telemetry TX       */

/* ---- baro_init() ---- */
void baro_init(int chip_id)
{
    s_chip_id = chip_id;
    s_data.pressure_pa      = 0;
    s_data.temp_x10         = 0;
    s_data.alt_cm           = 0;
    s_data.humidity_pct_x10 = -1;
    s_data.valid            = false;

    /*
     * Initialise timestamps so the first baro_tick() call fires immediately
     * (same convention as slope_estimator_init and steering_pid_init).
     */
    const uint32_t interval_ms = 1000u / BARO_READ_HZ;
    s_last_read_ms = (uint32_t)(-(uint32_t)interval_ms);
    s_last_tlm_ms  = (uint32_t)(-(uint32_t)(1000u / BARO_TLM_HZ));
}

/* ---- baro_tick() ---- */
void baro_tick(uint32_t now_ms)
{
    if (s_chip_id == 0) return;

    const uint32_t read_interval_ms = 1000u / BARO_READ_HZ;
    if ((now_ms - s_last_read_ms) < read_interval_ms) return;
    s_last_read_ms = now_ms;

    /* Read pressure (Pa) and temperature (°C × 10) */
    bmp280_read(&s_data.pressure_pa, &s_data.temp_x10);

    /* Compute pressure altitude: ISA formula, p0 = 101325 Pa */
    s_data.alt_cm = bmp280_pressure_to_alt_cm(s_data.pressure_pa);

    /* Humidity: BME280 (0x60) only; BMP280 returns -1 */
    s_data.humidity_pct_x10 = (s_chip_id == 0x60)
                                   ? bmp280_read_humidity()
                                   : (int16_t)-1;

    s_data.valid = true;

    /* Publish telemetry to Orin via JLink (JLINK_TLM_BARO = 0x8D) */
    const uint32_t tlm_interval_ms = 1000u / BARO_TLM_HZ;
    if ((now_ms - s_last_tlm_ms) >= tlm_interval_ms) {
        s_last_tlm_ms = now_ms;

        jlink_tlm_baro_t tlm;
        tlm.pressure_pa      = s_data.pressure_pa;
        tlm.temp_x10         = s_data.temp_x10;
        tlm.alt_cm           = s_data.alt_cm;
        tlm.humidity_pct_x10 = s_data.humidity_pct_x10;
        jlink_send_baro_tlm(&tlm);
    }
}

/* ---- baro_get() ---- */
bool baro_get(baro_data_t *out)
{
    if (!s_data.valid) return false;
    *out = s_data;
    return true;
}

/* ---- baro_get_alt_cm() ---- */
int32_t baro_get_alt_cm(void)
{
    return s_data.valid ? s_data.alt_cm : 0;
}