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

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

#include <hardware/pio.h>

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

using namespace ocularium;

static Adafruit_BME680 bme(&Wire1);
static VEML lux(Wire1);
static Adafruit_LSM6DSL lsm;

#define SCAN_I2C 0

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

static bringup::init_check STARTUP_CHECKS[] = {
    {
        .name = String("bme680"),
        .f = [] { return bme.begin(BME_ADDR); },
        .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_52_HZ);
                lsm.setGyroDataRate(LSM6DS_RATE_52_HZ);
                
                return true;
            };

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

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");

    bringup::boot_animation();
    delay(100);

    digitalWrite(LED_CAPTURING, HIGH);
}

void loop() {
    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();
    }

    static sensors_vec_t accel, gyro;

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

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

    struct
    {
        const char* name;
        float value;
    } readings[] = {
        {
            .name = "lux",
            .value = lux.lux(),
        },
        
        {
            .name = "temp",
            .value = bme.temperature,
        },
        {
            .name = "hum",
            .value = bme.humidity,
        },
        {
            .name = "pres",
            .value = static_cast<float>(bme.pressure),
        },
        {
            .name = "gas",
            .value = bme.humidity,
        },

        {
            .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();
}