//#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG

#include <esp_log.h>
#include <esp_err.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <driver/i2c.h>
#include <sys/time.h>
#include "voc_sensor.h"
#include "i2c_utils.h"
#include "data_report.h"

static const char *TAG = "voc";

#define VOC_I2C_NUM I2C_NUM_0
#define VOC_I2C_TO_MS 1000
#define VOC_SEMA_TO_MS 2000

// Config overrides for BSEC
#define BME68X_PERIOD_POLL  UINT32_C(5000)

#include "bme68x.h"
#include "bme68x_defs.h"
#include "bsec2.h"
#include "periph_init.h"
#include "settings.h"

struct sensor_itf {
    uint8_t dev_addr;
};

static struct sensor_itf gas_sensor_intf = {};
static struct bme68x_dev gas_sensor = {};
static struct bsec2 hBsec = {};

void bsec2_errormsg(const char *msg) {
    ESP_LOGE("BSEC", "%s", msg);
}

uint32_t bsec2_timestamp_millis() {
#if 0
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return tv.tv_sec*1000 + (tv.tv_usec / 1000000);
#else
    return pdTICKS_TO_MS(xTaskGetTickCount()); // This is no-op when using 1ms tick
#endif
}

static void user_delay_us(uint32_t period, void *intf_ptr) {
    (void) intf_ptr;

    uint32_t millis = period / 1000;
    uint32_t usec = period % 1000;

#if configTICK_RATE_HZ != 1000
    uint32_t remainder = millis % pdTICKS_TO_MS(1);
    millis -= remainder;
    usec += remainder * 1000;
#endif

    if (millis) {
        vTaskDelay(pdMS_TO_TICKS(millis));
    }
    if (usec) {
        ets_delay_us(usec);
    }
}

static BME68X_INTF_RET_TYPE user_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr) {
    ESP_LOGD(TAG, "BME680 i2c read");
    if (length == 0) {
        return BME68X_OK;
    }
    struct sensor_itf *itf = intf_ptr;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_reg_read(cmd, itf->dev_addr, reg_addr, reg_data, length);
    esp_err_t ret = i2c_master_cmd_begin(VOC_I2C_NUM, cmd, pdMS_TO_TICKS(VOC_I2C_TO_MS));
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C read error: %s, devaddr %x, reg %d, len %d", esp_err_to_name(ret), itf->dev_addr, reg_addr, length);
    }
    ESP_LOG_BUFFER_HEXDUMP(TAG, reg_data, length, ESP_LOG_DEBUG);
    return ret == ESP_OK ? BME68X_OK : BME68X_E_COM_FAIL;
}

static BME68X_INTF_RET_TYPE user_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr) {
    ESP_LOGD(TAG, "BME680 i2c write");
    if (length == 0) {
        return BME68X_OK;
    }
    struct sensor_itf *itf = intf_ptr;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_reg_write(cmd, itf->dev_addr, reg_addr, reg_data, length);
    esp_err_t ret = i2c_master_cmd_begin(VOC_I2C_NUM, cmd, pdMS_TO_TICKS(VOC_I2C_TO_MS));
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C write error: %s, devaddr %x, reg %d, len %d", esp_err_to_name(ret), itf->dev_addr, reg_addr, length);
    }
    return ret == ESP_OK ? BME68X_OK : BME68X_E_COM_FAIL;
}

static esp_err_t voc_init(void) {
    int8_t rslt;
    gas_sensor_intf.dev_addr = BME68X_I2C_ADDR_LOW;
    gas_sensor.intf_ptr = &gas_sensor_intf;
    gas_sensor.amb_temp = 25;
    gas_sensor.intf = BME68X_I2C_INTF;
    gas_sensor.read = user_i2c_read;
    gas_sensor.write = user_i2c_write;
    gas_sensor.delay_us = user_delay_us;

    for (int retry = 1; retry <= 3; retry++) {
        ESP_LOGD(TAG, "BME680 initializing (try %d)", retry);
        rslt = bme68x_init(&gas_sensor);
        if (rslt == BME68X_OK) {
            break;
        }
    }
    if (rslt != BME68X_OK) {
        ESP_LOGE(TAG, "Error from bme680_init: %d", rslt);
        return ESP_FAIL;
    }
    ESP_LOGI(TAG, "BME680 init OK");

    return ESP_OK;
}

static void new_data_callback(
        const struct bme68x_data *data,
        const bsecOutputs *outputs,
        const struct bsec2 *bsec
) {
    if (!outputs->nOutputs) {
        return;
    }

    struct data_report my_report = {};

    ESP_LOGD(TAG, "BSEC outputs: timestamp = %d", (int) (outputs->output[0].time_stamp / INT64_C(1000000)));
    for (uint8_t i = 0; i < outputs->nOutputs; i++) {
        const bsecData output = outputs->output[i];
        switch (output.sensor_id) {
            case BSEC_OUTPUT_IAQ:
                ESP_LOGD(TAG, "  IAQ = %f, accuracy %d", output.signal, (int) output.accuracy);
                my_report.iaq = output.signal;
                my_report.iaq_ready = (3 == (int) output.accuracy); // calibration finished
                break;
            case BSEC_OUTPUT_STATIC_IAQ:
                ESP_LOGD(TAG, "  STATIC_IAQ = %f, accuracy %d", output.signal, (int) output.accuracy);
                my_report.iaq_static = output.signal;
                break;
            case BSEC_OUTPUT_CO2_EQUIVALENT:
                ESP_LOGD(TAG, "  CO2_EQUIVALENT = %f, accuracy %d", output.signal, (int) output.accuracy);
                my_report.iaq_co2_ppm_equiv = output.signal;
                break;
            case BSEC_OUTPUT_BREATH_VOC_EQUIVALENT:
                ESP_LOGD(TAG, "  BREATH_VOC_EQUIVALENT = %f, accuracy %d", output.signal, (int) output.accuracy);
                my_report.iaq_voc_ppm_equiv = output.signal;
                break;
            case BSEC_OUTPUT_RAW_PRESSURE:
                ESP_LOGD(TAG, "  RAW_PRESSURE = %f", output.signal);
                my_report.pressure = output.signal;
                my_report.thpg_ready = true;
                break;
            case BSEC_OUTPUT_RAW_TEMPERATURE:
                ESP_LOGD(TAG, "  RAW_TEMPERATURE = %f", output.signal);
                my_report.temperature_raw = output.signal;
                my_report.thpg_ready = true;
                break;
            case BSEC_OUTPUT_RAW_HUMIDITY:
                ESP_LOGD(TAG, "  RAW_HUMIDITY = %f", output.signal);
                my_report.humidity_raw = output.signal;
                my_report.thpg_ready = true;
                break;
            case BSEC_OUTPUT_RAW_GAS:
                ESP_LOGD(TAG, "  RAW_GAS = %f", output.signal);
                my_report.gas_raw = output.signal;
                my_report.thpg_ready = true;
                break;
            case BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE:
                ESP_LOGD(TAG, "  SENSOR_HEAT_COMPENSATED_TEMPERATURE = %f", output.signal);
                my_report.temperature = output.signal;
                my_report.thpg_ready = true;
                break;
            case BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY:
                ESP_LOGD(TAG, "  SENSOR_HEAT_COMPENSATED_HUMIDITY = %f", output.signal);
                my_report.humidity = output.signal;
                my_report.thpg_ready = true;
                break;
            case BSEC_OUTPUT_STABILIZATION_STATUS:
                ESP_LOGD(TAG, "  STABILIZATION_STATUS status = %d", (int) output.signal);
                break;
            case BSEC_OUTPUT_RUN_IN_STATUS:
                ESP_LOGD(TAG, "  RUN_IN_STATUS = %d", (int) output.signal);
                break;
            default:
                break;
        }
    }

    if (pdPASS == xSemaphoreTake(g_mux_data_report, pdMS_TO_TICKS(1000))) {
        if (my_report.thpg_ready) {
            ESP_LOGI(TAG, "%.1f%%r.H, %.1f°C, %.0f Pa, gas %.0fR",
                     my_report.humidity, my_report.temperature, my_report.pressure, my_report.gas_raw);

            g_data_report.thpg_ready = true;
            g_data_report.thpg_timestamp = xTaskGetTickCount();
            g_data_report.humidity = my_report.humidity;
            g_data_report.humidity_raw = my_report.humidity_raw;
            g_data_report.temperature = my_report.temperature;
            g_data_report.temperature_raw = my_report.temperature_raw;
            g_data_report.pressure = my_report.pressure;
            g_data_report.gas_raw = my_report.gas_raw;
        } else {
            g_data_report.thpg_ready = false;
        }

        if (my_report.iaq_ready) {
            ESP_LOGI(TAG, "IAQ %.1f, IAQ_s %.1f, CO2eq %.1f ppm, VOCeq %.1f ppm",
                     my_report.iaq, my_report.iaq_static,
                     my_report.iaq_co2_ppm_equiv,
                     my_report.iaq_voc_ppm_equiv);

            g_data_report.iaq_ready = true;
            g_data_report.iaq_timestamp = xTaskGetTickCount();
            g_data_report.iaq = my_report.iaq;
            g_data_report.iaq_static = my_report.iaq_static;
            g_data_report.iaq_co2_ppm_equiv = my_report.iaq_co2_ppm_equiv;
            g_data_report.iaq_voc_ppm_equiv = my_report.iaq_voc_ppm_equiv;
        } else {
            g_data_report.iaq_ready = false;
        }

        xSemaphoreGive(g_mux_data_report);
    }
}

void voc_read_task(void *param) {
    ESP_LOGI(TAG, "VOC sensor init");

    if (ESP_OK != voc_init()) {
        ESP_LOGE(TAG, "Fail to init sensor!");
        goto abort;
    }

    int rv = bsec2_init(&hBsec, &gas_sensor);
    if (rv != 0) {
        ESP_LOGE(TAG, "Error in bsec init: %d", rv);
        goto abort;
    }

    bsecSensor sensorList[] = {
            BSEC_OUTPUT_IAQ,
            BSEC_OUTPUT_STATIC_IAQ,
            BSEC_OUTPUT_CO2_EQUIVALENT,
            BSEC_OUTPUT_BREATH_VOC_EQUIVALENT,

            BSEC_OUTPUT_RAW_TEMPERATURE,
            BSEC_OUTPUT_RAW_HUMIDITY,
            BSEC_OUTPUT_RAW_PRESSURE,
            BSEC_OUTPUT_RAW_GAS,

            BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE,
            BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY,

            BSEC_OUTPUT_STABILIZATION_STATUS,
            BSEC_OUTPUT_RUN_IN_STATUS,
    };

    rv = bsec2_updateSubscription(&hBsec,
                                  sensorList,
                                  sizeof(sensorList) / sizeof(bsecSensor),
                                  BSEC_SAMPLE_RATE_LP);

    if (false == rv) {
        ESP_LOGE(TAG, "Fail to update bsec subscriptions!");
        goto abort;
    }

    bsec2_attachCallback(&hBsec, new_data_callback);

    ESP_LOGI(TAG, "BSEC library version %d.%d.%d.%d",
             hBsec.version.major,
             hBsec.version.minor,
             hBsec.version.major_bugfix,
             hBsec.version.minor_bugfix);

    const uint32_t state_persist_time_ticks = 12 * 3600 * 1000;
    _Static_assert(configTICK_RATE_HZ == 1000, "1kHz tick");
    uint32_t last_state_persist = xTaskGetTickCount();

    ESP_LOGI(TAG, "Restore BSEC state");
    bsec2_setState(&hBsec, g_Settings.bsec_state);

    while (1) {
        rv = bsec2_run(&hBsec);
        if (false == rv) {
            ESP_LOGE(TAG, "Error in bsec run!");
        } else {
            uint32_t tickNow = xTaskGetTickCount();
            uint32_t elapsed = tickNow - last_state_persist;
            if (elapsed > state_persist_time_ticks) {
                ESP_LOGI(TAG, "Read & persist BSEC state");
                last_state_persist = tickNow;
                if (bsec2_getState(&hBsec, g_Settings.bsec_state)) {
                    settings_persist(SETTINGS_bsec_state);
                }
            }
        }
        vTaskDelay(pdMS_TO_TICKS(100));
    }

    abort:
    ESP_LOGE(TAG, "VOC task ends");
    vTaskDelete(NULL);
}