#include <Arduino.h>
#include <etl/vector.h>
#include <optional>

#include "channel.h"
#include "log.h"
#include "board.h"

#define ENABLE_SD 1

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


void setup1() {
    while (MEAS_CHANNEL == nullptr)
    {
        delay(5);
    }

    LOGGER.println("recorder core ready");
}

inline const char* name_for_meas(Measurement meas)
{
    switch (meas)
    {
    case Measurement::LUX:
        return "lux";
    case Measurement::TEMP:
        return "temp";
    case Measurement::HUM:
        return "hum";
    case Measurement::PRES:
        return "pres";
    case Measurement::GAS:
        return "gas";
    case Measurement::AX:
        return "ax";
    case Measurement::AY:
        return "ay";
    case Measurement::AZ:
        return "az";
    case Measurement::GX:
        return "gx";
    case Measurement::GY:
        return "gy";
    case Measurement::GZ:
        return "gz";

#if USE_BSEC
    case Measurement::CO2:
        return "co2";
    case Measurement::IAQ:
        return "iaq";
#endif

    default:
        return "unknown";
    }
}

void loop1() {
    static etl::vector<Message, 128> chunk;
    chunk.clear();

    while (!chunk.full())
    {
        Message read;
        if (!xQueueReceive(MEAS_CHANNEL, &read, portMAX_DELAY))
        {
            LOGGER.println("failed to receive message from channel");
            continue;
        }

        chunk.push_back(read);

        if (LOGGER)
        {
            for (int i = 0; i < static_cast<size_t>(Measurement::MAX); i++)
            {
                const auto name = name_for_meas(static_cast<Measurement>(i));
                const auto& value = read.measurement[i];

                // This awkwardness is to support the fact that the RTT implementation
                // doesn't support formatting floats and instead silently fails -- manually
                // format first, then printf the string.
                char buf[32];
                snprintf(buf, sizeof(buf), "%s:%f,", name, value);

                LOGGER.printf("%s", buf);
            }

            LOGGER.println();
        }
    }

#if !ENABLE_SD
    chunk.clear();
    return;
#endif

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

    // Pin is negated
    if (digitalRead(CARD_DETECT))
    {
        sd_connected = false;
        analogWrite(LED_CAPTURING, 0);
        return;
    }

    if (millis() - sd_last_connect > sd_period && !sd_connected)
    {
        sd_last_connect = millis();

        if (!sd.begin(SD_CS))
        {
            LOGGER.println("failed connection to sd card");
            sd_connected = false;
            analogWrite(LED_CAPTURING, 0);

            return;
        }

        sd_connected = true;
        analogWrite(LED_CAPTURING, 24);
    }

    if (!sd_connected)
    {
        return;
    }

    auto data = sd.open("data.csv", O_CREAT | O_WRITE | O_APPEND);
    if (data.size() == 0)
    {
        data.write("uptime_ms,");

        for (int i = 0; i < static_cast<size_t>(Measurement::MAX); i++)
        {
            const auto name = name_for_meas(static_cast<Measurement>(i));
            data.printf("%s,", name);
        }

        data.print('\n');
    }

    for (const auto& m : chunk)
    {
        data.printf("%d,", m.uptime);

        for (const auto &value : m.measurement)
        {
            data.printf("%f,", value);
        }

        data.print('\n');
    }

    data.close();
#endif
}