#include "FreeRTOS.h"
#include "gpio.h"
#include "main.h"
#include "semphr.h"
#include "spi.h"
#include "task.h"

#include <cstring>

#include "BME68x-Sensor-API/bme68x.h"
#include "BSEC/bsec_interface.h"
#include "oled-driver/Renderer.hpp"

extern QueueHandle_t spiMutex;
extern void waitForSpiFinished();
extern Renderer renderer;

extern void initDisplay();

constexpr auto MaximumChars = 22 * 4;
char buffer[MaximumChars];

constexpr auto SpiPeripherie = &hspi2;
uint8_t txBuffer[512 + 1];

constexpr auto temperatureOffset = 7.0f;

struct bme68x_dev bmeSensor;
struct bme68x_conf bmeConf;
struct bme68x_heatr_conf bmeHeaterConf;
struct bme68x_data bmeData[3];

uint32_t delayInUs;
uint8_t numberOfData;

constexpr auto ProfileLength = 1;

// Heater temperature in degree Celsius
uint16_t temperatureProfile[ProfileLength] = {320};

// Heating duration in milliseconds
uint16_t durationProfile[ProfileLength] = {150};

constexpr uint8_t numberRequestedVirtualSensors = 4;
bsec_sensor_configuration_t requestedVirtualSensors[numberRequestedVirtualSensors];

float iaq, rawTemperature, pressure, rawHumidity, gasResistance, stabStatus, runInStatus,
    temperature, humidity, staticIaq, co2Equivalent, breathVocEquivalent, compGasValue,
    gasPercentage;

uint8_t iaqAccuracy, staticIaqAccuracy, co2Accuracy, breathVocAccuracy, compGasAccuracy,
    gasPercentageAcccuracy;

// uint8_t bsecState[BSEC_MAX_STATE_BLOB_SIZE];
// uint8_t workBuffer[BSEC_MAX_WORKBUFFER_SIZE];

void setChipSelect(bool state)
{
    HAL_GPIO_WritePin(VocSensorCS_GPIO_Port, VocSensorCS_Pin,
                      state ? GPIO_PIN_RESET : GPIO_PIN_SET);
}

// SPI read function map
BME68X_INTF_RET_TYPE bme68x_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *)
{
    xSemaphoreTake(spiMutex, portMAX_DELAY);

    setChipSelect(true);
    HAL_SPI_Transmit_DMA(SpiPeripherie, &reg_addr, 1);
    waitForSpiFinished();
    HAL_SPI_Receive_DMA(SpiPeripherie, reg_data, len);
    waitForSpiFinished();
    setChipSelect(false);

    xSemaphoreGive(spiMutex);

    return 0;
}

// SPI write function map
BME68X_INTF_RET_TYPE bme68x_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len,
                                      void *)
{
    if (len > 512)
        return 1;

    txBuffer[0] = reg_addr;
    std::memcpy(&txBuffer[1], reg_data, len);

    xSemaphoreTake(spiMutex, portMAX_DELAY);

    setChipSelect(true);
    HAL_SPI_Transmit_DMA(SpiPeripherie, const_cast<uint8_t *>(txBuffer), len + 1);
    waitForSpiFinished();
    setChipSelect(false);

    xSemaphoreGive(spiMutex);

    return 0;
}

// Delay function maps
void bme68x_delay_us(uint32_t period, void *)
{
    vTaskDelay(period / 1000);
}

int8_t bme68x_spi_init(struct bme68x_dev *bme)
{
    int8_t rslt = BME68X_OK;

    if (bme != NULL)
    {
        bme->read = bme68x_spi_read;
        bme->write = bme68x_spi_write;
        bme->intf = BME68X_SPI_INTF;

        bme->delay_us = bme68x_delay_us;
        bme->amb_temp =
            25; /* The ambient temperature in deg C is used for defining the heater temperature */
    }
    else
    {
        rslt = BME68X_E_NULL_PTR;
    }

    return rslt;
}

void bmeSensorInit()
{
    bme68x_spi_init(&bmeSensor);
    bme68x_init(&bmeSensor);

    bme68x_get_conf(&bmeConf, &bmeSensor);
    bmeConf.os_hum = BME68X_OS_16X;
    bmeConf.os_temp = BME68X_OS_2X;
    bmeConf.os_pres = BME68X_OS_1X;
    bmeConf.filter = BME68X_FILTER_OFF;
    bmeConf.odr = BME68X_ODR_NONE;
    bme68x_set_conf(&bmeConf, &bmeSensor);

    bmeHeaterConf.enable = BME68X_ENABLE;
    bmeHeaterConf.heatr_temp_prof = temperatureProfile;
    bmeHeaterConf.heatr_dur_prof = durationProfile;
    bmeHeaterConf.profile_len = ProfileLength;
    bme68x_set_heatr_conf(BME68X_SEQUENTIAL_MODE, &bmeHeaterConf, &bmeSensor);

    bme68x_set_op_mode(BME68X_SEQUENTIAL_MODE, &bmeSensor);

    bsec_init();

    // Change 3 virtual sensors (switch IAQ and raw temperature -> on / pressure -> off
    requestedVirtualSensors[0].sensor_id = BSEC_OUTPUT_IAQ;
    requestedVirtualSensors[0].sample_rate = BSEC_SAMPLE_RATE_CONTINUOUS;
    requestedVirtualSensors[1].sensor_id = BSEC_OUTPUT_CO2_EQUIVALENT;
    requestedVirtualSensors[1].sample_rate = BSEC_SAMPLE_RATE_CONTINUOUS;
    requestedVirtualSensors[2].sensor_id = BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE;
    requestedVirtualSensors[2].sample_rate = BSEC_SAMPLE_RATE_CONTINUOUS;
    requestedVirtualSensors[3].sensor_id = BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY;
    requestedVirtualSensors[3].sample_rate = BSEC_SAMPLE_RATE_CONTINUOUS;

    // Allocate a struct for the returned physical sensor settings
    bsec_sensor_configuration_t requiredSensorSettings[BSEC_MAX_PHYSICAL_SENSOR];
    uint8_t numberRequiredSensorSettings = BSEC_MAX_PHYSICAL_SENSOR;

    // Call bsec_update_subscription() to enable/disable the requested virtual sensors
    bsec_update_subscription(requestedVirtualSensors, numberRequestedVirtualSensors,
                             requiredSensorSettings, &numberRequiredSensorSettings);
}

void bmeRun()
{
    delayInUs = bme68x_get_meas_dur(BME68X_SEQUENTIAL_MODE, &bmeConf, &bmeSensor) +
                (bmeHeaterConf.heatr_dur_prof[0] * 1000);
    vTaskDelay(delayInUs / 1000);

    auto status = bme68x_get_data(BME68X_SEQUENTIAL_MODE, bmeData, &numberOfData, &bmeSensor);
    if (status != 0)
    {
        __asm("bkpt");
    }
}

void bsecRun()
{
    /*
    auto status = bsec_set_state(state, BSEC_MAX_STATE_BLOB_SIZE, workBuffer, sizeof(workBuffer));

    if (status == BSEC_OK)
    {
        for (uint32_t i = 0; i < BSEC_MAX_STATE_BLOB_SIZE; i++)
        {
            bsecState[i] = state[i];
        }
        validBsecState = true;
    }
    */

    if (!(bmeData[numberOfData - 1].status & BME68X_NEW_DATA_MSK))
    {
        __asm("bkpt");
        return;
    }

    bsec_input_t inputs[BSEC_MAX_PHYSICAL_SENSOR];
    uint8_t nInputs = 0, nOutputs = 0;
    int64_t currentTimeInNs = xTaskGetTickCount() * int64_t(1000) * int64_t(1000);

    inputs[nInputs].sensor_id = BSEC_INPUT_TEMPERATURE;
    inputs[nInputs].signal = bmeData[numberOfData - 1].temperature / 100.0f;
    inputs[nInputs].time_stamp = currentTimeInNs;
    nInputs++;

    inputs[nInputs].sensor_id = BSEC_INPUT_HUMIDITY;
    inputs[nInputs].signal = bmeData[numberOfData - 1].humidity / 1000.0f;
    inputs[nInputs].time_stamp = currentTimeInNs;
    nInputs++;

    inputs[nInputs].sensor_id = BSEC_INPUT_PRESSURE;
    inputs[nInputs].signal = bmeData[numberOfData - 1].pressure;
    inputs[nInputs].time_stamp = currentTimeInNs;
    nInputs++;

    inputs[nInputs].sensor_id = BSEC_INPUT_GASRESISTOR;
    inputs[nInputs].signal = bmeData[numberOfData - 1].gas_resistance;
    inputs[nInputs].time_stamp = currentTimeInNs;
    nInputs++;

    inputs[nInputs].sensor_id = BSEC_INPUT_HEATSOURCE;
    inputs[nInputs].signal = temperatureOffset;
    inputs[nInputs].time_stamp = currentTimeInNs;
    nInputs++;

    nOutputs = BSEC_NUMBER_OUTPUTS;
    bsec_output_t outputs[BSEC_NUMBER_OUTPUTS];

    auto status = bsec_do_steps(inputs, nInputs, outputs, &nOutputs);
    if (status != BSEC_OK)
    {
        return;
    }

    // zeroOutputs();

    if (nOutputs > 0)
    {
        auto outputTimestamp = outputs[0].time_stamp / 1000000; /* Convert from ns to ms */

        for (uint8_t i = 0; i < nOutputs; i++)
        {
            switch (outputs[i].sensor_id)
            {
            case BSEC_OUTPUT_IAQ:
                iaq = outputs[i].signal;
                iaqAccuracy = outputs[i].accuracy;
                break;
            case BSEC_OUTPUT_STATIC_IAQ:
                staticIaq = outputs[i].signal;
                staticIaqAccuracy = outputs[i].accuracy;
                break;
            case BSEC_OUTPUT_CO2_EQUIVALENT:
                co2Equivalent = outputs[i].signal;
                co2Accuracy = outputs[i].accuracy;
                break;
            case BSEC_OUTPUT_BREATH_VOC_EQUIVALENT:
                breathVocEquivalent = outputs[i].signal;
                breathVocAccuracy = outputs[i].accuracy;
                break;
            case BSEC_OUTPUT_RAW_TEMPERATURE:
                rawTemperature = outputs[i].signal;
                break;
            case BSEC_OUTPUT_RAW_PRESSURE:
                pressure = outputs[i].signal;
                break;
            case BSEC_OUTPUT_RAW_HUMIDITY:
                rawHumidity = outputs[i].signal;
                break;
            case BSEC_OUTPUT_RAW_GAS:
                gasResistance = outputs[i].signal;
                break;
            case BSEC_OUTPUT_STABILIZATION_STATUS:
                stabStatus = outputs[i].signal;
                break;
            case BSEC_OUTPUT_RUN_IN_STATUS:
                runInStatus = outputs[i].signal;
                break;
            case BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE:
                temperature = outputs[i].signal;
                break;
            case BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY:
                humidity = outputs[i].signal;
                break;
            case BSEC_OUTPUT_COMPENSATED_GAS:
                compGasValue = outputs[i].signal;
                compGasAccuracy = outputs[i].accuracy;
                break;
            case BSEC_OUTPUT_GAS_PERCENTAGE:
                gasPercentage = outputs[i].signal;
                gasPercentageAcccuracy = outputs[i].accuracy;
                break;
            default:
                break;
            }
        }
    }
}

void printBmeSensorData()
{
    renderer.clearAll();

    snprintf(buffer, MaximumChars,
             "%d°C, %luhPa, %d%%\nIAQ: %d, Accuracy: %d\nCO2: %dppm\n%d, %d, %d - %lukOhm",
             static_cast<int>(temperature),            //
             bmeData[numberOfData - 1].pressure / 100, //
             static_cast<int>(humidity),               //
             static_cast<int>(iaq),                    //
             iaqAccuracy,                              //
             static_cast<int>(co2Equivalent),          //
             bmeData[numberOfData - 1].status,         //
             bmeData[numberOfData - 1].gas_index,      //
             bmeData[numberOfData - 1].gas_wait,       //
             bmeData[numberOfData - 1].gas_resistance / 1000);

    renderer.print({0, 0}, buffer);
    renderer.render();
}

//--------------------------------------------------------------------------------------------------
extern "C" void sensorTask(void *)
{
    initDisplay();
    bmeSensorInit();

    while (1)
    {
        bmeRun();
        bsecRun();
        printBmeSensorData();
    }
}