#include "bringup.h"
#include "board.h"

#include <Wire.h>

using namespace ocularium;

void bringup::startup_checks(const etl::span<bringup::init_check> &checks)
{
    auto success = true;
    auto fault_indication = false;
    
    do
    {
        success = true;
        
        for (auto &[name, f, succeeded] : checks)
        {
            if (succeeded)
                continue;

            Serial.print("checking " + name + "... ");

            succeeded = f();
            success = success && succeeded;

            if (succeeded) Serial.println("ok");
            else Serial.println("bad");
        }

        if (!success)
        {
            fault_indication = !fault_indication;
            digitalWrite(LED_FAULT, fault_indication);
            delay(25);
        }
    } while (!success);

    digitalWrite(LED_FAULT, LOW);
}

void bringup::init_buses()
{
    Wire1.setSCL(SCL_PIN);
    Wire1.setSDA(SDA_PIN);
    Wire1.setClock(100000);
    Wire1.begin();

    SD_SPI.setCS(SD_CS);
    SD_SPI.setMOSI(SD_MOSI);
    SD_SPI.setMISO(SD_MISO);
    SD_SPI.setSCK(SD_SCK);
    SD_SPI.begin();

    LSM_SPI.setCS(LSM_CS);
    LSM_SPI.setMOSI(LSM_MOSI);
    LSM_SPI.setMISO(LSM_MISO);
    LSM_SPI.setSCK(LSM_SCK);
    LSM_SPI.begin();
}

void bringup::init_pins()
{
    pinMode(CARD_DETECT, INPUT);
    pinMode(LED_FAULT, OUTPUT);
    pinMode(LED_CAPTURING, OUTPUT);
    pinMode(LED_OTHER, OUTPUT);
    pinMode(LED_STORAGE, OUTPUT);
    pinMode(LSM_INT1, INPUT);
    pinMode(LSM_INT2, INPUT);
}

void bringup::boot_animation()
{
    constexpr pin_size_t chase[] = {
        LED_STORAGE,
        LED_OTHER,
        LED_FAULT,
        LED_CAPTURING,
    };

    for (auto i = 0; i < 4; i++)
    {
        for (const auto pin: chase)
        {
            digitalWrite(pin, HIGH);
            delay(75);
            digitalWrite(pin, LOW);
        }
    }

    delay(100);

    for (auto i = 0; i < 2; i++)
    {
        for (const auto pin : chase)
        {
            digitalWrite(pin, HIGH);
        }
        delay(100);
        for (const auto pin : chase)
        {
            digitalWrite(pin, LOW);
        }
        delay(100);
    }
}