#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>

#include <Adafruit_LSM6DS.h>
#include <Adafruit_LSM6DSL.h>

#include <FreeRTOS.h>
#include <task.h>
#include <message_buffer.h>
#include <queue.h>

#include <hardware/pio.h>

#include "i2c_scan.h"
#include "veml.h"
#include "board.h"
#include "bringup.h"
#include "log.h"
#include "channel.h"

QueueHandle_t MEAS_CHANNEL;
static StaticQueue_t MEAS_QUEUE;
static uint8_t MEAS_STORAGE[MEAS_CHAN_LEN * sizeof(Message)];

#define USE_BSEC 0

#if USE_RTT
RTTStream rtt;
#endif

#if USE_BSEC
#include <bsec.h>
static Bsec bme;
#else
#include <Adafruit_BME680.h>
static Adafruit_BME680 bme(&Wire1);
#endif

using namespace ocularium;

static VEML lux(Wire1);
static Adafruit_LSM6DSL lsm;

#define SCAN_I2C 0

static bringup::init_check STARTUP_CHECKS[] = {
    {
        .name = String("bme680"),
        .f = []
        {
#if USE_BSEC
            bme.begin(BME_ADDR, Wire1);

            bsec_virtual_sensor_t sensors[] = {
                BSEC_OUTPUT_IAQ,
                BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY,
                BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE,
                BSEC_OUTPUT_RAW_PRESSURE,
                BSEC_OUTPUT_GAS_PERCENTAGE,
                BSEC_OUTPUT_CO2_EQUIVALENT,
            };
            bme.updateSubscription(sensors, ETL_ARRAY_SIZE(sensors), BSEC_SAMPLE_RATE_CONT);
            return true;
#else
            return bme.begin(BME_ADDR);
#endif
        },
        .succeeded = false,
    },
    {
        .name = String("veml7700"),
        .f = []{ return lux.init(veml::Config {
            .gain = veml::AmbientLightGain::Quarter,
        }); },
        .succeeded = false,
    },
    {
        .name = String("lsm6dsm"),
        .f = []
        {
            if (lsm.begin_SPI(LSM_CS, &LSM_SPI))
            {
                lsm.reset();

                lsm.setAccelDataRate(LSM6DS_RATE_104_HZ);
                lsm.setGyroDataRate(LSM6DS_RATE_104_HZ);

                return true;
            };

            return false;
        },
        .succeeded = false,
    },
};

#if USE_BSEC
struct bme_data
{
    float temperature, humidity, pressure, iaq, co2, gas;
};

static uint8_t bme_storage[2 * sizeof(bme_data)];
static StaticQueue_t bme_buffer;
static QueueHandle_t bme_buffer_handle;
#endif

void setup() {
#if USE_RTT
    rtt.trimDownBufferFull();
    rtt.trimUpBufferFull();
#endif

    LOGGER.begin(115200);
    LOGGER.println("boot");

    bringup::init_buses();
    bringup::init_pins();

#if SCAN_I2C
    LOGGER.println("scanning...");
    scan(Wire1);
#endif

    bringup::startup_checks(STARTUP_CHECKS);

    LOGGER.println("sensors initialized");

#if USE_BSEC
    [[noreturn]] void bme_task(void* params);

    bme_buffer_handle = xQueueCreateStatic(2, sizeof(bme_data), bme_storage, &bme_buffer);

    static StaticTask_t bsec_task_buffer;
    static StackType_t bsec_stack[2048];
    xTaskCreateStatic(bme_task, "bme", 2048, nullptr, tskIDLE_PRIORITY, bsec_stack, &bsec_task_buffer);
#endif

    MEAS_CHANNEL = xQueueCreateStatic(32, sizeof(Message), MEAS_STORAGE, &MEAS_QUEUE);

    bringup::boot_animation();
}


#if USE_BSEC
[[noreturn]] void bme_task(void* params)
{
    while (true)
    {
        LOGGER.println("bme run start");
        const auto status = bme.run();
        LOGGER.println("bme run end");

        if (status)
        {
            bme_data data {
                .temperature = bme.temperature,
                .humidity = bme.humidity,
                .pressure = bme.pressure,
                .iaq = bme.iaq,
                .co2 = bme.co2Equivalent,
                .gas = bme.gasPercentage,
            };

            xQueueSend(bme_buffer_handle, &data, 0);
        }

        delay(max(bme.nextCall - millis(), 0));
    }
}
#endif

constexpr float ODR_HZ = 25;
constexpr float ODR_PERIOD = 1.0 / ODR_HZ;
constexpr uint32_t ODR_PERIOD_MILLIS = static_cast<uint32_t>(ODR_PERIOD * 1000.f);

void loop() {
    static uint32_t loop_top = 0;

#if USE_BSEC
    static bme_data bme_data;
    // do not wait
    xQueueReceiveFromISR(bme_buffer_handle, &bme_data, nullptr);
    auto& bme_data_source = bme_data;

#else
    static uint32_t bme_target_millis = 0;

    if (millis() >= bme_target_millis)
    {
        if (bme_target_millis != 0 && !bme.endReading())
        {
            LOGGER.println("bme read error!");
        }

        bme_target_millis = bme.beginReading();
    }

    auto& bme_data_source = bme;
#endif

    const auto lux_value = lux.lux();
    analogWrite(LED_STORAGE, map(static_cast<int>(lux_value), 200, 1000, 0, 255));

    static sensors_vec_t accel, gyro;

    if (lsm.gyroscopeAvailable())
    {
        lsm.readGyroscope(gyro.x, gyro.y, gyro.z);
    }

    if (lsm.accelerationAvailable())
    {
        lsm.readAcceleration(accel.x, accel.y, accel.z);

        const auto norm = sqrtf(accel.x * accel.x + accel.y * accel.y + accel.z * accel.z);

        // small rolling average to reduce the effects of noise
        constexpr auto WINDOW = 10;
        static float norm_hist[WINDOW] = {0.0};
        for (int i = 0; i < WINDOW - 1; i++)
            norm_hist[i] = norm_hist[i + 1];
        norm_hist[WINDOW - 1] = norm;

        auto norm_avg = 0.f;
        for (const float i : norm_hist)
        {
            norm_avg += i;
        }
        norm_avg /= static_cast<float>(WINDOW);

        constexpr auto NEUTRAL = 1.98f;
        constexpr auto RANGE = 8.f;

        auto compensated = abs(norm_avg - NEUTRAL) / RANGE;
        compensated *= 5;    // more sensitivity (heuristic)

        const auto pin_val = etl::clamp(static_cast<int>(compensated * 255), 0, 255);

        analogWrite(LED_OTHER, pin_val);
    }

    const Message msg = {
        .uptime = millis(),
        .measurement = {
            lux_value,
            bme_data_source.temperature,
            bme_data_source.humidity,
            static_cast<float>(bme_data_source.pressure),
#if USE_BSEC
            bme_data_source.gas,
#else
            static_cast<float>(bme.gas_resistance),
#endif

            accel.x,
            accel.y,
            accel.z,

            gyro.x,
            gyro.y,
            gyro.z,

#if USE_BSEC
            bme_data_source.iaq,
            bme_data_source.co2,
#endif
        },
    };

    if (!xQueueSend(MEAS_CHANNEL, &msg, 0))
    {
        LOGGER.println("queue was full, dropping measurement!");
    }

    const auto now = millis();
    const auto next = loop_top + ODR_PERIOD_MILLIS;
    loop_top = next;
    if (next < now) return;
    delay(next - now);
}