#include <string.h>

#include "sensors_common.h"
#include "dht_bme280.h"

#define IMS_BME280_CONCAT_BYTES(msb, lsb) (((uint16_t)msb << 8) | (uint16_t)lsb)

#ifdef IMS_CFG_DHT_BME280_ENABLE_FLOAT
static double ims_bme280_compensate_T_double(ims_bme280_t *bme, int32_t adc_T);
static double ims_bme280_compensate_P_double(ims_bme280_t *bme, int32_t adc_P);
static double ims_bme280_compensate_H_double(ims_bme280_t *bme, int32_t adc_H);
#else
static int32_t  ims_bme280_compensate_T_int32(ims_bme280_t *bme, int32_t adc_T);
static uint32_t ims_bme280_compensate_P_int64(ims_bme280_t *bme, int32_t adc_P);
static uint32_t ims_bme280_compensate_H_int32(ims_bme280_t *bme, int32_t adc_H);
#endif

static ims_ret_t ims_bme280_read_register(ims_bme280_t *bme, uint8_t reg, uint8_t *data, uint16_t len) {
    ims_i2c_xfer_desc_t xfer_desc = {
        .tx_size = 1U,
        .tx_data = &reg,
        .rx_size = len,
        .rx_data = data,
    };

    return bme->cb.i2c_xfer(bme->user_data, bme->i2c_addr, &xfer_desc);
}

static ims_ret_t ims_bme280_write_register(ims_bme280_t *bme, uint8_t reg, uint8_t data) {
    uint8_t             tx_buf[2] = {reg, data};
    ims_i2c_xfer_desc_t xfer_desc = {
        .tx_size = 2U,
        .tx_data = tx_buf,
        .rx_size = 0U,
        .rx_data = NULL,
    };

    return bme->cb.i2c_xfer(bme->user_data, bme->i2c_addr, &xfer_desc);
}

static ims_ret_t ims_bme280_reset(ims_bme280_t *bme) {
    return ims_bme280_write_register(bme, 0xE0, 0xB6);
}

static ims_ret_t ims_bme280_read_trim_data(ims_bme280_t *bme) {
    ims_ret_t ret;
    uint8_t   rx_buf[2];
    ret = ims_bme280_read_register(bme, 0x88, rx_buf, 0x02);  // T1
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_T1 = IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x8A, rx_buf, 0x02);  // T2
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_T2 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x8C, rx_buf, 0x02);  // T3
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_T3 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x8E, rx_buf, 0x02);  // P1
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P1 = IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x90, rx_buf, 0x02);  // P2
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P2 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x92, rx_buf, 0x02);  // P3
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P3 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x94, rx_buf, 0x02);  // P4
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P4 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x96, rx_buf, 0x02);  // P5
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P5 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x98, rx_buf, 0x02);  // P5
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P6 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x9A, rx_buf, 0x02);  // P7
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P7 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x9C, rx_buf, 0x02);  // P8
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P8 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0x9E, rx_buf, 0x02);  // P9
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_P9 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0xA1, rx_buf, 0x01);  // H1
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_H1 = rx_buf[0];

    ret = ims_bme280_read_register(bme, 0xE1, rx_buf, 0x02);  // H2
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_H2 = (int16_t)IMS_BME280_CONCAT_BYTES(rx_buf[1], rx_buf[0]);

    ret = ims_bme280_read_register(bme, 0xE3, rx_buf, 0x01);  // H3
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_H3 = (uint8_t)rx_buf[0];

    ret = ims_bme280_read_register(bme, 0xE4, rx_buf, 0x02);  // H4
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_H4 = (int16_t)(((int8_t)rx_buf[0] << 0x04) | (rx_buf[1] & 0x0F));

    ret = ims_bme280_read_register(bme, 0xE5, rx_buf, 0x02);  // H5
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_H5 = (int16_t)(((int8_t)rx_buf[1] << 0x04) | (rx_buf[0] >> 0x04));

    ret = ims_bme280_read_register(bme, 0xE7, rx_buf, 0x01);  // H6
    if (ret == IMS_FAIL) return ret;
    bme->trim.dig_H6 = (int8_t)rx_buf[0];
    return IMS_SUCCESS;
}

static ims_ret_t ims_bme280_measure_raw(ims_bme280_t *bme, uint32_t *raw_t, uint32_t *raw_p, uint32_t *raw_h) {
    uint8_t measure_data[8];
    uint8_t status = 0;
    uint8_t ctrl_meas;
    uint8_t loop_count = 0;
    if (ims_bme280_read_register(bme, 0xF4, &ctrl_meas, 0x01) != IMS_SUCCESS) return IMS_FAIL;

    ims_bme280_write_register(bme, 0xF4, ctrl_meas | IMS_BME280_MODE_FORCED);
    do {
        if (ims_bme280_read_register(bme, 0xF3, &status, 0x01) != IMS_SUCCESS) return IMS_FAIL;
        loop_count++;
        bme->cb.delay(bme->user_data, 100);
    } while (status & 0x08 && (loop_count < 12));

    if (ims_bme280_read_register(bme, 0xF7, measure_data, 0x08) != IMS_SUCCESS) return IMS_FAIL;
    *raw_p =
        ((uint32_t)measure_data[0] << 12) | ((uint32_t)measure_data[1] << 0x04) | ((uint32_t)measure_data[2] >> 0x04);
    *raw_t =
        ((uint32_t)measure_data[3] << 12) | ((uint32_t)measure_data[4] << 0x04) | ((uint32_t)measure_data[5] >> 0x04);
    *raw_h = ((uint32_t)measure_data[6] << 8) | ((uint32_t)measure_data[7]);

    return IMS_SUCCESS;
}

ims_ret_t ims_bme280_init(ims_bme280_t *bme) {
    if (!bme) return IMS_FAIL;
    if (ims_bme280_reset(bme) == IMS_FAIL) return IMS_FAIL;
    bme->cb.delay(bme->user_data, 100);
    if (ims_bme280_read_trim_data(bme) == IMS_FAIL) return IMS_FAIL;
    return IMS_SUCCESS;
}

ims_ret_t ims_bme280_preset_config(ims_bme280_config_t *config, ims_bme280_mode_preset_t preset) {
    switch (preset) {
        case IMS_BME280_PRESET_WEATHER:
            config->mode   = IMS_BME280_MODE_FORCED;
            config->osrs_t = IMS_BME280_OSRS_1;
            config->osrs_p = IMS_BME280_OSRS_1;
            config->osrs_h = IMS_BME280_OSRS_1;
            config->filter = IMS_BME280_FILTER_OFF;
            break;
        case IMS_BME280_PRESET_HUMIDITY:
            config->mode   = IMS_BME280_MODE_FORCED;
            config->osrs_t = IMS_BME280_OSRS_1;
            config->osrs_p = IMS_BME280_OSRS_SKIP;
            config->osrs_h = IMS_BME280_OSRS_1;
            config->filter = IMS_BME280_FILTER_OFF;
            break;
        case IMS_BME280_PRESET_INDOOR_NAV:
            config->mode   = IMS_BME280_MODE_NORMAL;
            config->osrs_t = IMS_BME280_OSRS_2;
            config->osrs_p = IMS_BME280_OSRS_16;
            config->osrs_h = IMS_BME280_OSRS_1;
            config->filter = IMS_BME280_FILTER_16;
            break;
        case IMS_BME280_PRESET_GAMING:
            config->mode   = IMS_BME280_MODE_NORMAL;
            config->osrs_t = IMS_BME280_OSRS_1;
            config->osrs_p = IMS_BME280_OSRS_4;
            config->osrs_h = IMS_BME280_OSRS_SKIP;
            config->filter = IMS_BME280_FILTER_4;
            break;
    }
    return IMS_SUCCESS;
}

ims_ret_t ims_bme280_apply_config(ims_bme280_t *bme, ims_bme280_config_t *config) {
    uint8_t reg_config       = config->filter << 0x02;
    uint8_t reg_ctrl_measure = (uint8_t)((config->osrs_t << 0x05) | (config->osrs_p << 0x02) | config->mode);
    uint8_t reg_ctrl_hum     = config->osrs_h;
    ims_bme280_write_register(bme, 0xF5, reg_config);
    ims_bme280_write_register(bme, 0xF2, reg_ctrl_hum);
    ims_bme280_write_register(bme, 0xF4, reg_ctrl_measure);
    return IMS_SUCCESS;
}

#ifdef IMS_CFG_DHT_BME280_ENABLE_FLOAT

ims_ret_t ims_bme280_measure_float(ims_bme280_t *bme, ims_bme280_result_float_t *result) {
    uint32_t raw_P = 0x00;
    uint32_t raw_T = 0x00;
    uint32_t raw_H = 0x00;

    if (ims_bme280_measure_raw(bme, &raw_T, &raw_P, &raw_H) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    result->temperature = ims_bme280_compensate_T_double(bme, raw_T);
    result->pressure    = ims_bme280_compensate_P_double(bme, raw_P);
    result->humidity    = ims_bme280_compensate_H_double(bme, raw_H);
    return IMS_SUCCESS;
}

static double ims_bme280_compensate_T_double(ims_bme280_t *bme, int32_t adc_T) {
    double var1, var2, T;
    var1 = (((double)adc_T) / 16384.0 - ((double)bme->trim.dig_T1) / 1024.0) * ((double)bme->trim.dig_T2);
    var2 = ((((double)adc_T) / 131072.0 - ((double)bme->trim.dig_T1) / 8192.0) *
            (((double)adc_T) / 131072.0 - ((double)bme->trim.dig_T1) / 8192.0)) *
           ((double)bme->trim.dig_T3);
    bme->t_fine = (int32_t)(var1 + var2);
    T           = (var1 + var2) / 5120.0;
    return T;
}

static double ims_bme280_compensate_P_double(ims_bme280_t *bme, int32_t adc_P) {
    double var1, var2, p;
    var1 = ((double)bme->t_fine / 2.0) - 64000.0;
    var2 = var1 * var1 * ((double)bme->trim.dig_P6) / 32768.0;
    var2 = var2 + var1 * ((double)bme->trim.dig_P5) * 2.0;
    var2 = (var2 / 4.0) + (((double)bme->trim.dig_P4) * 65536.0);
    var1 = (((double)bme->trim.dig_P3) * var1 * var1 / 524288.0 + ((double)bme->trim.dig_P2) * var1) / 524288.0;
    var1 = (1.0 + var1 / 32768.0) * ((double)bme->trim.dig_P1);
    if (var1 == 0.0) {
        return 0;  // avoid exception caused by division by zero
    }
    p    = 1048576.0 - (double)adc_P;
    p    = (p - (var2 / 4096.0)) * 6250.0 / var1;
    var1 = ((double)bme->trim.dig_P9) * p * p / 2147483648.0;
    var2 = p * ((double)bme->trim.dig_P8) / 32768.0;
    p    = p + (var1 + var2 + ((double)bme->trim.dig_P7)) / 16.0;
    return p;
}

static double ims_bme280_compensate_H_double(ims_bme280_t *bme, int32_t adc_H) {
    double var_H;
    var_H = (((double)bme->t_fine) - 76800.0);
    var_H = (adc_H - (((double)bme->trim.dig_H4) * 64.0 + ((double)bme->trim.dig_H5) / 16384.0 * var_H)) *
            (((double)bme->trim.dig_H2) / 65536.0 *
             (1.0 + ((double)bme->trim.dig_H6) / 67108864.0 * var_H *
                        (1.0 + ((double)bme->trim.dig_H3) / 67108864.0 * var_H)));
    var_H = var_H * (1.0 - ((double)bme->trim.dig_H1) * var_H / 524288.0);
    if (var_H > 100.0)
        var_H = 100.0;
    else if (var_H < 0.0)
        var_H = 0.0;
    return var_H;
}
#else

ims_ret_t ims_bme280_measure_int32(ims_bme280_t *bme, ims_bme280_result_int32_t *result) {
    uint32_t raw_P = 0x00;
    uint32_t raw_T = 0x00;
    uint32_t raw_H = 0x00;

    if (ims_bme280_measure_raw(bme, &raw_T, &raw_P, &raw_H) != IMS_SUCCESS) {
        return IMS_FAIL;
    }

    result->temperature = ims_bme280_compensate_T_int32(bme, raw_T);
    result->pressure    = ims_bme280_compensate_P_int64(bme, raw_P);
    result->humidity    = ims_bme280_compensate_H_int32(bme, raw_H);
    return IMS_SUCCESS;
}

static int32_t ims_bme280_compensate_T_int32(ims_bme280_t *bme, int32_t adc_T) {
    int32_t var1, var2, T;
    var1 = ((((adc_T >> 3) - ((int32_t)bme->trim.dig_T1 << 1))) * ((int32_t)bme->trim.dig_T2)) >> 11;
    var2 = (((((adc_T >> 4) - ((int32_t)bme->trim.dig_T1)) * ((adc_T >> 4) - ((int32_t)bme->trim.dig_T1))) >> 12) *
            ((int32_t)bme->trim.dig_T3)) >>
           14;
    bme->t_fine = var1 + var2;
    T           = (bme->t_fine * 5 + 128) >> 8;
    return T;
}

static uint32_t ims_bme280_compensate_P_int64(ims_bme280_t *bme, int32_t adc_P) {
    int64_t var1, var2, p;
    var1 = ((int64_t)bme->t_fine) - 128000;
    var2 = var1 * var1 * (int64_t)bme->trim.dig_P6;
    var2 = var2 + ((var1 * (int64_t)bme->trim.dig_P5) << 17);
    var2 = var2 + (((int64_t)bme->trim.dig_P4) << 35);
    var1 = ((var1 * var1 * (int64_t)bme->trim.dig_P3) >> 8) + ((var1 * (int64_t)bme->trim.dig_P2) << 12);
    var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)bme->trim.dig_P1) >> 33;
    if (var1 == 0) {
        return 0;  // avoid exception caused by division by zero
    }
    p    = 1048576 - adc_P;
    p    = (((p << 31) - var2) * 3125) / var1;
    var1 = (((int64_t)bme->trim.dig_P9) * (p >> 13) * (p >> 13)) >> 25;
    var2 = (((int64_t)bme->trim.dig_P8) * p) >> 19;
    p    = ((p + var1 + var2) >> 8) + (((int64_t)bme->trim.dig_P7) << 4);
    return (uint32_t)p;
}

static uint32_t ims_bme280_compensate_H_int32(ims_bme280_t *bme, int32_t adc_H) {
    int32_t v_x1_u32r;
    v_x1_u32r = (bme->t_fine - ((int32_t)76800));
    v_x1_u32r = (((((adc_H << 14) - (((int32_t)bme->trim.dig_H4) << 20) - (((int32_t)bme->trim.dig_H5) * v_x1_u32r)) +
                   ((int32_t)16384)) >>
                  15) *
                 (((((((v_x1_u32r * ((int32_t)bme->trim.dig_H6)) >> 10) *
                      (((v_x1_u32r * ((int32_t)bme->trim.dig_H3)) >> 11) + ((int32_t)32768))) >>
                     10) +
                    ((int32_t)2097152)) *
                       ((int32_t)bme->trim.dig_H2) +
                   8192) >>
                  14));
    v_x1_u32r = (v_x1_u32r - (((((v_x1_u32r >> 15) * (v_x1_u32r >> 15)) >> 7) * ((int32_t)bme->trim.dig_H1)) >> 4));
    v_x1_u32r = (v_x1_u32r < 0 ? 0 : v_x1_u32r);
    v_x1_u32r = (v_x1_u32r > 419430400 ? 419430400 : v_x1_u32r);
    return (uint32_t)(v_x1_u32r >> 12);
}
#endif