/*
 * bmp280.c — BMP280/BME280 barometer driver (I2C1, shared bus)
 *
 * Probes 0x76 first, then 0x77. Requires i2c1_init() before bmp280_init().
 * Returns chip_id on success (0x58=BMP280, 0x60=BME280), negative if absent.
 *
 * All HAL_I2C_Mem_Read/Write calls use 100ms timeouts — init cannot hang
 * indefinitely even if the I2C bus is stuck or the breakout is absent.
 *
 * BME280 (chip_id 0x60): bmp280_read_humidity() returns %RH × 10.
 * Call bmp280_read() first to refresh t_fine, then bmp280_read_humidity().
 */
#include "bmp280.h"
#include "i2c1.h"
#include <math.h>

static uint16_t s_addr;
static int      s_chip_id;  /* 0x58=BMP280, 0x60=BME280, 0=none */

/* Shared temp/pressure calibration */
static uint16_t dig_T1;
static int16_t  dig_T2, dig_T3;
static uint16_t dig_P1;
static int16_t  dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9;
static int32_t  t_fine;  /* updated by bmp280_read(); used by bmp280_read_humidity() */

/* BME280-only humidity calibration (chip_id 0x60) */
static uint8_t  dig_H1;
static int16_t  dig_H2;
static uint8_t  dig_H3;
static int16_t  dig_H4, dig_H5;
static int8_t   dig_H6;

static uint8_t i2c_read(uint8_t reg) {
    uint8_t val = 0;
    HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, &val, 1, 100);
    return val;
}

static void i2c_read_burst(uint8_t reg, uint8_t *buf, uint8_t len) {
    HAL_I2C_Mem_Read(&hi2c1, s_addr, reg, 1, buf, len, 100);
}

static void i2c_write(uint8_t reg, uint8_t val) {
    HAL_I2C_Mem_Write(&hi2c1, s_addr, reg, 1, &val, 1, 100);
}

static int try_init(uint16_t addr) {
    s_addr = addr;
    uint8_t id = i2c_read(0xD0);
    if (id != 0x58 && id != 0x60) return -(int)id;
    s_chip_id = (int)id;

    /* Temp/pressure calibration (0x88–0x9D, identical layout on BMP280 and BME280) */
    uint8_t cal[26];
    i2c_read_burst(0x88, cal, 26);
    dig_T1 = (uint16_t)(cal[1] << 8 | cal[0]);
    dig_T2 = (int16_t) (cal[3] << 8 | cal[2]);
    dig_T3 = (int16_t) (cal[5] << 8 | cal[4]);
    dig_P1 = (uint16_t)(cal[7] << 8 | cal[6]);
    dig_P2 = (int16_t) (cal[9] << 8 | cal[8]);
    dig_P3 = (int16_t) (cal[11] << 8 | cal[10]);
    dig_P4 = (int16_t) (cal[13] << 8 | cal[12]);
    dig_P5 = (int16_t) (cal[15] << 8 | cal[14]);
    dig_P6 = (int16_t) (cal[17] << 8 | cal[16]);
    dig_P7 = (int16_t) (cal[19] << 8 | cal[18]);
    dig_P8 = (int16_t) (cal[21] << 8 | cal[20]);
    dig_P9 = (int16_t) (cal[23] << 8 | cal[22]);

    if (id == 0x60) {
        /* BME280: humidity calibration.
         *   dig_H1 : 0xA1         (uint8)
         *   dig_H2 : 0xE1–0xE2   (int16,  LSB first)
         *   dig_H3 : 0xE3         (uint8)
         *   dig_H4 : 0xE4[7:0] | 0xE5[3:0]  (int12)
         *   dig_H5 : 0xE5[7:4]  | 0xE6[7:0] (int12)
         *   dig_H6 : 0xE7         (int8)
         */
        dig_H1 = i2c_read(0xA1);
        uint8_t hcal[7];
        i2c_read_burst(0xE1, hcal, 7);
        dig_H2 = (int16_t)((hcal[1] << 8) | hcal[0]);
        dig_H3 = hcal[2];
        dig_H4 = (int16_t)((hcal[3] << 4) | (hcal[4] & 0x0F));
        dig_H5 = (int16_t)((hcal[5] << 4) | (hcal[4] >> 4));
        dig_H6 = (int8_t)hcal[6];

        /* ctrl_hum (0xF2) MUST be written before ctrl_meas (0xF4) — hardware req */
        i2c_write(0xF2, 0x05);  /* osrs_h = ×16 */
    }

    i2c_write(0xF5, 0x00);  /* config: standby=0.5ms, filter=off */
    i2c_write(0xF4, 0xB7);  /* ctrl_meas: osrs_t=×16, osrs_p=×16, normal mode */

    return (int)id;
}

int bmp280_init(void) {
    int ret = try_init(0x76 << 1);
    if (ret > 0) return ret;
    return try_init(0x77 << 1);
}

void bmp280_read(int32_t *pressure_pa, int16_t *temp_x10) {
    uint8_t buf[6];
    i2c_read_burst(0xF7, buf, 6);

    int32_t adc_P = (int32_t)((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4));
    int32_t adc_T = (int32_t)((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4));

    /* Temperature compensation (BME280/BMP280 datasheet Section 4.2.3) */
    int32_t v1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
    int32_t v2 = (((((adc_T >> 4) - (int32_t)dig_T1) *
                    ((adc_T >> 4) - (int32_t)dig_T1)) >> 12) * (int32_t)dig_T3) >> 14;
    t_fine = v1 + v2;
    *temp_x10 = (int16_t)((t_fine * 5 + 128) >> 8);  /* 0.1 °C */

    /* Pressure compensation */
    int64_t p1 = ((int64_t)t_fine) - 128000;
    int64_t p2 = p1 * p1 * (int64_t)dig_P6;
    p2 += (p1 * (int64_t)dig_P5) << 17;
    p2 += ((int64_t)dig_P4) << 35;
    p1 = ((p1 * p1 * (int64_t)dig_P3) >> 8) + ((p1 * (int64_t)dig_P2) << 12);
    p1 = ((((int64_t)1 << 47) + p1)) * ((int64_t)dig_P1) >> 33;
    if (p1 == 0) { *pressure_pa = 0; return; }
    int64_t p = 1048576 - adc_P;
    p = (((p << 31) - p2) * 3125) / p1;
    p1 = ((int64_t)dig_P9 * (p >> 13) * (p >> 13)) >> 25;
    p2 = ((int64_t)dig_P8 * p) >> 19;
    *pressure_pa = (int32_t)(((p + p1 + p2) >> 8) + ((int64_t)dig_P7 << 4)) / 256;
}

/*
 * BME280-only humidity readout. MUST be called after bmp280_read() (uses t_fine).
 *
 * Compensation: BME280 datasheet section 4.2.3 integer formula.
 * Result is Q22.10 fixed-point: 1024 units = 1 %RH.
 *
 * Returns humidity in %RH × 10 (e.g. 500 = 50.0 %RH).
 * Returns -1 if chip is BMP280 (no humidity sensor).
 */
int16_t bmp280_read_humidity(void) {
    if (s_chip_id != 0x60) return -1;

    uint8_t hbuf[2];
    i2c_read_burst(0xFD, hbuf, 2);
    int32_t adc_H = (int32_t)((hbuf[0] << 8) | hbuf[1]);

    /* BME280 datasheet section 4.2.3 — floating-point compensation.
     * Single-precision float is hardware-accelerated on STM32F7 (FPv5-SP FPU).
     * Called at 50 Hz — negligible overhead.
     */
    float var_H = ((float)t_fine) - 76800.0f;
    var_H = (adc_H - (((float)dig_H4) * 64.0f + ((float)dig_H5) / 16384.0f * var_H)) *
            (((float)dig_H2) / 65536.0f *
             (1.0f + ((float)dig_H6) / 67108864.0f * var_H *
              (1.0f + ((float)dig_H3) / 67108864.0f * var_H)));
    var_H *= (1.0f - (float)dig_H1 * var_H / 524288.0f);
    if (var_H > 100.0f) var_H = 100.0f;
    if (var_H < 0.0f)   var_H = 0.0f;
    return (int16_t)(var_H * 10.0f + 0.5f);  /* %RH × 10, rounded */
}

int32_t bmp280_pressure_to_alt_cm(int32_t pressure_pa) {
    /* Barometric formula: h = 44330 * (1 - (p/p0)^(1/5.255)) metres */
    float ratio = (float)pressure_pa / 101325.0f;
    float alt_m = 44330.0f * (1.0f - powf(ratio, 0.1902949f));
    return (int32_t)(alt_m * 100.0f);  /* cm */
}
