#include <string.h>
#include "bsec2.h"

//#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG

#include <esp_log.h>
#include <inttypes.h>
static const char *TAG = "bsec";

/* Private funcs - prototypes */

/**
 * @brief Reads the data from the BME68x sensor and process it
 * @param currTimeNs: Current time in ns
 * @return true if there are new outputs. false otherwise
 */
static bool bsec2_processData(struct bsec2 *self, int64_t currTimeNs, const struct bme68x_data *data);

/**
 * @brief Set the BME68x sensor configuration to forced mode
 */
static void bsec2_setBme68xConfigForced(struct bsec2 *self);

/**
 * @brief Set the BME68x sensor configuration to parallel mode
 */
static void bsec2_setBme68xConfigParallel(struct bsec2 *self);


/* Public funcs impl */

int bsec2_init(struct bsec2 *self, struct bme68x_dev *dev) {
    self->ovfCounter = 0;
    self->lastMillis = 0;
    self->status = BSEC_OK;
    self->extTempOffset = 0.0f;
    self->opMode = BME68X_SLEEP_MODE;
    self->newDataCallback = NULL;
    self->sensor = dev;
    memset(&self->version, 0, sizeof(self->version));
    memset(&self->bmeConf, 0, sizeof(self->bmeConf));
    memset(&self->outputs, 0, sizeof(self->outputs));

    // The sensor must be inited and OK by the time this func is called!

    // Begin
    self->status = bsec_init();
    if (self->status != BSEC_OK) {
        return -1;
    }
    self->status = bsec_get_version(&self->version);
    if (self->status != BSEC_OK) {
        return -2;
    }
    memset(&self->bmeConf, 0, sizeof(self->bmeConf));
    memset(&self->outputs, 0, sizeof(self->outputs));
    return 0;
}

bool bsec2_updateSubscription(struct bsec2 *self, const bsecSensor *sensorList, uint8_t nSensors, float sampleRate) {
    bsec_sensor_configuration_t virtualSensors[BSEC_NUMBER_OUTPUTS], sensorSettings[BSEC_MAX_PHYSICAL_SENSOR];
    uint8_t nSensorSettings = BSEC_MAX_PHYSICAL_SENSOR;

    for (uint8_t i = 0; i < nSensors; i++) {
        virtualSensors[i].sensor_id = sensorList[i];
        virtualSensors[i].sample_rate = sampleRate;
    }

    /* Subscribe to library virtual sensors outputs */
    self->status = bsec_update_subscription(virtualSensors, nSensors, sensorSettings, &nSensorSettings);
    if (self->status != BSEC_OK) {
        return false;
    }

    return true;
}

bool bsec2_run(struct bsec2 *self) {
    int64_t ms = bsec2_getTimeMs(self);
    int64_t currTimeNs = ms * INT64_C(1000000);

    //ESP_LOGD(TAG, "%"PRIi64" ms = %"PRIi64" ns, nextcall %"PRIi64, ms, currTimeNs, self->bmeConf.next_call);

    uint8_t lastOpMode = self->opMode;

    // set new opmode to what bsec wants
    self->opMode = self->bmeConf.op_mode;

    if (currTimeNs >= self->bmeConf.next_call) {
        ESP_LOGD(TAG, "bsec runs");

        /* Provides the information about the current sensor configuration that is
           necessary to fulfill the input requirements, eg: operation mode, timestamp
           at which the sensor data shall be fetched etc */
        self->status = bsec_sensor_control(currTimeNs, &self->bmeConf);
        if (self->status != BSEC_OK) {
            return false;
        }

        switch (self->bmeConf.op_mode) {
            case BME68X_FORCED_MODE:
                bsec2_setBme68xConfigForced(self);
                break;
            case BME68X_PARALLEL_MODE:
                if (self->opMode != self->bmeConf.op_mode) {
                    bsec2_setBme68xConfigParallel(self);
                }
                break;

            case BME68X_SLEEP_MODE:
                if (self->opMode != self->bmeConf.op_mode) {
                    self->sensor_status = bme68x_set_op_mode(BME68X_SLEEP_MODE, self->sensor);
                    self->opMode = BME68X_SLEEP_MODE;
                }
                break;
        }

        if (self->sensor_status < BME68X_OK) {
            bsec2_errormsg("bsec2 sensor error");
            return false;
        }

        struct bme68x_data sensorData[3];
        struct bme68x_data *pdata;

        if (self->bmeConf.trigger_measurement && self->bmeConf.op_mode != BME68X_SLEEP_MODE) {
            uint8_t nfields;
            self->sensor_status = bme68x_get_data(self->opMode, sensorData, &nfields, self->sensor);

            if (self->sensor_status == BME68X_OK && nfields > 0) {
                if (lastOpMode == BME68X_FORCED_MODE) {
                    pdata = &sensorData[0];
                    if (pdata->status & BME68X_GASM_VALID_MSK) {
                        if (!bsec2_processData(self, currTimeNs, pdata)) {
                            return false;
                        }
                    }
                } else {
                    for (int i = 0; i < nfields; i++) {
                        pdata = &sensorData[i];
                        if (pdata->status & BME68X_GASM_VALID_MSK) {
                            if (!bsec2_processData(self, currTimeNs, pdata)) {
                                return false;
                            }
                        }
                    }
                }
            }
        }
    }
    return true;
}

bsecData bsec2_getData(struct bsec2 *self, bsecSensor id) {
    bsecData emp =
            {0};
    for (uint8_t i = 0; i < self->outputs.nOutputs; i++)
        if (id == self->outputs.output[i].sensor_id)
            return self->outputs.output[i];
    return emp;
}

/**
 * @brief Function to get the state of the algorithm to save to non-volatile memory
 * @param state			: Pointer to a memory location, to hold the state
 * @return	true for success, false otherwise
 */
bool bsec2_getState(struct bsec2 *self, uint8_t *state) {
    uint32_t n_serialized_state = BSEC_MAX_STATE_BLOB_SIZE;

    self->status = bsec_get_state(0, state, BSEC_MAX_STATE_BLOB_SIZE, self->workBuffer, BSEC_MAX_WORKBUFFER_SIZE,
                                  &n_serialized_state);
    if (self->status != BSEC_OK) {
        bsec2_errormsg("bsec2_getState: error from bsec_get_state");
        return false;
    }
    return true;
}

/**
 * @brief Function to set the state of the algorithm from non-volatile memory
 * @param state			: Pointer to a memory location that contains the state
 * @return	true for success, false otherwise
 */
bool bsec2_setState(struct bsec2 *self, uint8_t *state) {
    self->status = bsec_set_state(state, BSEC_MAX_STATE_BLOB_SIZE, self->workBuffer, BSEC_MAX_WORKBUFFER_SIZE);
    if (self->status != BSEC_OK) {
        bsec2_errormsg("bsec2_setState: error from bsec_set_state");
        return false;
    }

    memset(&self->bmeConf, 0, sizeof(self->bmeConf));

    return true;
}

/**
 * @brief Function to retrieve the current library configuration
 * @param config    : Pointer to a memory location, to hold the serialized config blob, must hold BSEC_MAX_PROPERTY_BLOB_SIZE
 * @return	true for success, false otherwise
 */
bool bsec2_getConfig(struct bsec2 *self, uint8_t *config) {
    uint32_t n_serialized_settings = 0;

    self->status = bsec_get_configuration(0, config, BSEC_MAX_PROPERTY_BLOB_SIZE, self->workBuffer, BSEC_MAX_WORKBUFFER_SIZE, &n_serialized_settings);
    if (self->status != BSEC_OK) {
        bsec2_errormsg("bsec2_getConfig: error from bsec_get_configuration");
        return false;
    }

    return true;
}

/**
 * @brief Function to set the configuration of the algorithm from memory
 * @param state			: Pointer to a memory location that contains the configuration
 * @return	true for success, false otherwise
 */
bool bsec2_setConfig(struct bsec2 *self, const uint8_t *config) {
    self->status = bsec_set_configuration(config, BSEC_MAX_PROPERTY_BLOB_SIZE, self->workBuffer, BSEC_MAX_WORKBUFFER_SIZE);
    if (self->status != BSEC_OK) {
        bsec2_errormsg("bsec2_setConfig: error from bsec_set_configuration");
        return false;
    }

    // copied from arduino lib, but why?
    memset(&self->bmeConf, 0, sizeof(self->bmeConf));
    return true;
}

/**
 * @brief Function to calculate an int64_t timestamp in milliseconds
 */
int64_t bsec2_getTimeMs(struct bsec2 *self) {
    uint64_t timeMs = bsec2_timestamp_millis();
    if (self->lastMillis > timeMs) /* An overflow occurred */
    {
        self->lastMillis = timeMs;
        self->ovfCounter++;
    }
    self->lastMillis = timeMs;
    return (int64_t) timeMs + (self->ovfCounter * INT64_C(0xFFFFFFFF));
}

/* Private funcs impl */

static bool bsec2_processData(struct bsec2 *self, int64_t currTimeNs, const struct bme68x_data *data) {

    bsec_input_t inputs[BSEC_MAX_PHYSICAL_SENSOR]; /* Temp, Pres, Hum & Gas */
    uint8_t nInputs = 0;
    /* Checks all the required sensor inputs, required for the BSEC library for the requested outputs */
    if (BSEC_CHECK_INPUT(self->bmeConf.process_data, BSEC_INPUT_HEATSOURCE)) {
        inputs[nInputs].sensor_id = BSEC_INPUT_HEATSOURCE;
        inputs[nInputs].signal = self->extTempOffset;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(self->bmeConf.process_data, BSEC_INPUT_TEMPERATURE)) {
#ifdef BME68X_USE_FPU
        inputs[nInputs].sensor_id = BSEC_INPUT_TEMPERATURE;
#else
        inputs[nInputs].sensor_id = BSEC_INPUT_TEMPERATURE / 100.0f;
#endif
        inputs[nInputs].signal = data->temperature;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(self->bmeConf.process_data, BSEC_INPUT_HUMIDITY)) {
#ifdef BME68X_USE_FPU
        inputs[nInputs].sensor_id = BSEC_INPUT_HUMIDITY;
#else
        inputs[nInputs].sensor_id = BSEC_INPUT_HUMIDITY / 1000.0f;
#endif
        inputs[nInputs].signal = data->humidity;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(self->bmeConf.process_data, BSEC_INPUT_PRESSURE)) {
        inputs[nInputs].sensor_id = BSEC_INPUT_PRESSURE;
        inputs[nInputs].signal = data->pressure;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(self->bmeConf.process_data, BSEC_INPUT_GASRESISTOR) &&
        (data->status & BME68X_GASM_VALID_MSK)) {
        inputs[nInputs].sensor_id = BSEC_INPUT_GASRESISTOR;
        inputs[nInputs].signal = data->gas_resistance;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(self->bmeConf.process_data, BSEC_INPUT_PROFILE_PART) &&
        (data->status & BME68X_GASM_VALID_MSK)) {
        inputs[nInputs].sensor_id = BSEC_INPUT_PROFILE_PART;
        inputs[nInputs].signal = (self->opMode == BME68X_FORCED_MODE) ? 0 : (float) data->gas_index;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }

    if (nInputs > 0) {

        self->outputs.nOutputs = BSEC_NUMBER_OUTPUTS;
        memset(self->outputs.output, 0, sizeof(self->outputs.output));

        /* Processing of the input signals and returning of output samples is performed by bsec_do_steps() */
        self->status = bsec_do_steps(inputs, nInputs, self->outputs.output, &self->outputs.nOutputs);

        if (self->status != BSEC_OK) {
            bsec2_errormsg("bsec2_processData: error from bsec_do_steps");
            return false;
        }

        if (self->newDataCallback) {
            self->newDataCallback(data, &self->outputs, self);
        }
    }
    return true;
}

static void bsec2_setBme68xConfigForced(struct bsec2 *self) {
    struct bme68x_conf conf;
    struct bme68x_heatr_conf hconf;

    self->sensor_status = bme68x_get_conf(&conf, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigForced: error from bme68x_get_conf");
        return;
    }

    conf.os_hum = self->bmeConf.humidity_oversampling;
    conf.os_pres = self->bmeConf.pressure_oversampling;
    conf.os_temp = self->bmeConf.temperature_oversampling;

    self->sensor_status = bme68x_set_conf(&conf, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigForced: error from bme68x_set_conf");
        return;
    }

    hconf.enable = BME68X_ENABLE;
    hconf.heatr_dur = self->bmeConf.heater_duration;
    hconf.heatr_temp = self->bmeConf.heater_temperature;
    // the other parameters are not needed for forced mode

    self->sensor_status = bme68x_set_heatr_conf(BME68X_FORCED_MODE, &hconf, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigForced: error from bme68x_set_heatr_conf");
        return;
    }

    self->sensor_status = bme68x_set_op_mode(BME68X_FORCED_MODE, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigForced: error from bme68x_set_op_mode");
        return;
    }

    self->opMode = BME68X_FORCED_MODE;
}

static void bsec2_setBme68xConfigParallel(struct bsec2 *self) {
    uint16_t sharedHeaterDur = 0;

    struct bme68x_conf conf;
    struct bme68x_heatr_conf hconf;

    self->sensor_status = bme68x_get_conf(&conf, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigParallel: error from bme68x_get_conf");
        return;
    }

    conf.os_hum = self->bmeConf.humidity_oversampling;
    conf.os_pres = self->bmeConf.pressure_oversampling;
    conf.os_temp = self->bmeConf.temperature_oversampling;

    self->sensor_status = bme68x_set_conf(&conf, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigParallel: error from bme68x_set_conf");
        return;
    }

    sharedHeaterDur = BSEC_TOTAL_HEAT_DUR - (bme68x_get_meas_dur(BME68X_PARALLEL_MODE, &conf, self->sensor) / INT64_C(1000));

    hconf.enable = BME68X_ENABLE;
    hconf.shared_heatr_dur = sharedHeaterDur;
    hconf.heatr_dur = self->bmeConf.heater_duration;
    hconf.heatr_temp = self->bmeConf.heater_temperature;
    hconf.heatr_temp_prof = self->bmeConf.heater_temperature_profile;
    hconf.heatr_dur_prof = self->bmeConf.heater_duration_profile;
    hconf.profile_len = self->bmeConf.heater_profile_len;

    self->sensor_status = bme68x_set_heatr_conf(BME68X_PARALLEL_MODE, &hconf, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigParallel: error from bme68x_set_heatr_conf");
        return;
    }

    self->sensor_status = bme68x_set_op_mode(BME68X_PARALLEL_MODE, self->sensor);
    if (BME68X_OK != self->sensor_status) {
        bsec2_errormsg("bsec2_setBme68xConfigParallel: error from bme68x_set_op_mode");
        return;
    }

    self->opMode = BME68X_PARALLEL_MODE;
}