#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"

#define USE_BSEC 0

#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
#define ENABLE_SD 0

#define SDCARD_SPI SD_SPI
#include <SD.h>
static SDClass sd;

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() {
    Serial.begin(115200);
    Serial.println("boot");

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

#if SCAN_I2C
    Serial.println("scanning...");
    scan(Wire1);
#endif
    
    bringup::startup_checks(STARTUP_CHECKS);

    Serial.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
    
    bringup::boot_animation();
    delay(100);
}


#if USE_BSEC
[[noreturn]] void bme_task(void* params)
{
    while (true)
    {
        Serial.println("bme run start");
        const auto status = bme.run();
        Serial.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

void loop() {
#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())
        {
            Serial.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));

    const bool card_detected = !digitalRead(CARD_DETECT);
    analogWrite(LED_CAPTURING, card_detected ? 24 : 0);
    
    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);

        // Serial.printf("norm: %.2f, compensated: %.2f, pin_val: %d\n", norm, compensated, pin_val);
        
        analogWrite(LED_OTHER, pin_val);
    }

#if ENABLE_SD
    constexpr auto sd_period = 50;
    static unsigned int sd_last_connect = 0;
    static bool sd_connected = false;

    if (!card_detected)
    {
        sd_connected = false;
    }
    
    if (millis() - sd_last_connect > sd_period && card_detected)
    {
        sd_last_connect = millis();
        
        if (!sd.begin(SD_CS))
        {
            Serial.println("failed connection to sd card");
            sd_connected = false;
        } else
        {
            sd_connected = true;
        }
    }
#endif

    struct
    {
        const char* name;
        float value;
    } readings[] = {
        {
            .name = "lux",
            .value = lux_value,
        },
        
        {
            .name = "temp",
            .value = bme_data_source.temperature,
        },
        {
            .name = "hum",
            .value = bme_data_source.humidity,
        },
        {
            .name = "pres",
            .value = static_cast<float>(bme_data_source.pressure),
        },
#if USE_BSEC
        {
            .name = "iaq",
            .value = bme_data_source.iaq,
        },
        {
            .name = "co2",
            .value = bme_data_source.co2,
        },
        {
            .name = "gas",
            .value = bme_data_source.gas,
        },
#else
        {
            .name = "gas",
            .value = static_cast<float>(bme.gas_resistance),
        },
#endif

        {
            .name = "ax",
            .value = accel.x,
        },
        {
            .name = "ay",
            .value = accel.y,
        },
        {
            .name = "az",
            .value = accel.z,
        },
        
        {
            .name = "gx",
            .value = gyro.x,
        },
        {
            .name = "gy",
            .value = gyro.y,
        },
        {
            .name = "gz",
            .value = gyro.z,
        },
    };

    for (const auto &[name, value] : readings)
    {
        Serial.printf("%s:%f,", name, value);
    }
    
    Serial.println();

#if ENABLE_SD
    if (sd_connected) {
        auto data = sd.open("data.csv", O_CREAT | O_WRITE | O_APPEND);
        
        data.write(String(millis()).c_str());
        
        for (const auto &[name, value] : readings)
        {
            data.write(String(value).c_str());
            data.write(",");
        }
        
        data.write("\n");
        data.close();
    }
#endif
}