#define DEBUGLOG_DEFAULT_LOG_LEVEL_DEBUG

// Arduino Libraries
#include <Arduino.h>
#include <DebugLog.h>
#include <DebugLogEnable.h>
#include <SPI.h>
#include <WiFi.h>
#include <ArduinoJson.h>

// Definitions
#include <tasks.h>
#include <devices.h>
#include <datasave.h>
#include <webserver.h>
#include <ui.h>
#include <led.h>

#define CH_A_ENABLE
#define CH_B_ENABLE
#define CH_A_RT_ENABLE
#define CH_B_RT_ENABLE
// #define I2C_ENABLE
// #define WEB_ENABLE

// Debug Defines
#define WIFI_SSID "AstroRotaxMonitor"
#define WIFI_PASSWORD "maledettirotax"
#define PSRAM_MAX 1024
#define QUEUE_MAX 32

void setup()
{
    Serial.begin(115200);
    delay(250);
    Serial.setTimeout(5000);

    // Setup Logger
    LOG_ATTACH_SERIAL(Serial);
    LOG_SET_LEVEL(DebugLogLevel::LVL_DEBUG);

    // Print Processor Info
    LOG_DEBUG("ESP32 Chip:", ESP.getChipModel());
    if (psramFound())
    {
        LOG_DEBUG("ESP32 PSram Found");
        LOG_DEBUG("ESP32 PSram:", ESP.getPsramSize());
        psramInit();
    }
    LOG_DEBUG("ESP32 Flash:", ESP.getFlashChipSize());
    LOG_DEBUG("ESP32 Heap:", ESP.getHeapSize());
    LOG_DEBUG("ESP32 Sketch:", ESP.getFreeSketchSpace());

// Init Wifi station
#ifdef WEB_ENABLE
    LOG_INFO("Initializing WiFi...");
    WiFi.mode(WIFI_AP);
    IPAddress local_IP(10, 11, 12, 1);
    IPAddress gateway(10, 11, 12, 1);
    IPAddress subnet(255, 255, 255, 0);
    WiFi.softAPConfig(local_IP, gateway, subnet);
    WiFi.setTxPower(WIFI_POWER_5dBm); // reduce wifi power
    if (WiFi.softAP(WIFI_SSID, WIFI_PASSWORD))
    {
        LOG_INFO("WiFi AP Mode Started");
        LOG_INFO("Wifi SSID:", WIFI_SSID);
        LOG_INFO("Wifi Password:", WIFI_PASSWORD);
        LOG_INFO("WiFi IP:" + WiFi.softAPIP().toString());
    }
    else
    {
        LOG_ERROR("Failed to start WiFi AP Mode");
        LOG_ERROR("5 seconds to restart...");
        vTaskDelay(pdMS_TO_TICKS(5000));
        esp_restart();
    }
#endif

    // Initialize Interrupt pins on PICKUP detectors
    initTriggerPinsInputs();
    // Initialize Interrupt pins on SPARK detectors
    initSparkPinInputs();
}

////////////////////// MAIN LOOP //////////////////////
void loop()
{
    // global variables
    RGBled led;
    led.setBrightness(0.025f);
    led.setStatus(RGBled::LedStatus::INIT);

    Devices dev;
    bool running = true;
    std::mutex fs_mutex;
    LITTLEFSGuard fsGuard;

    //////// INIT SPI INTERFACES ////////
    bool spiA_ok = true;
    bool spiB_ok = true;
    //////// INIT SPI INTERFACES ////////
    LOG_DEBUG("Init SPI Interfaces");
#ifdef CH_A_ENABLE
    LOG_DEBUG("Begin Init SPI_A");
    SPIClass SPI_A(HSPI);
    spiA_ok = SPI_A.begin(SPI_A_SCK, SPI_A_MISO, SPI_A_MOSI);
    SPI_A.setDataMode(SPI_MODE1); // ADS1256 requires SPI mode 1
    LOG_DEBUG("Init SPI_A -> OK");
    delay(500);
    LOG_DEBUG("Begin Init ADC_A");
    ADS1256 ADC_A(ADC_A_DRDY, ADS1256::PIN_UNUSED, ADS1256::PIN_UNUSED, ADC_A_CS, 2.5, &SPI_A);
    ADC_A.InitializeADC();
    ADC_A.setPGA(PGA_1);
    ADC_A.setDRATE(DRATE_7500SPS);
    dev.m_adc_a = &ADC_A;
    dev.m_spi_a = &SPI_A;
    LOG_DEBUG("Init ADC_A -> OK");
    delay(1000);
#endif

#ifdef CH_B_ENABLE
    LOG_DEBUG("Begin Init SPI_B");
    SPIClass SPI_B(FSPI);
    spiB_ok = SPI_B.begin(SPI_B_SCK, SPI_B_MISO, SPI_B_MOSI);
    SPI_B.setDataMode(SPI_MODE1); // ADS1256 requires SPI mode 1
    LOG_DEBUG("Init SPI_B -> OK");
    delay(500);
    LOG_DEBUG("Begin Init ADC_B");
    ADS1256 ADC_B(ADC_B_DRDY, ADS1256::PIN_UNUSED, ADS1256::PIN_UNUSED, ADC_B_CS, 2.5, &SPI_B);
    ADC_B.InitializeADC();
    ADC_B.setPGA(PGA_1);
    ADC_B.setDRATE(DRATE_7500SPS);
    dev.m_adc_b = &ADC_B;
    dev.m_spi_b = &SPI_B;
    LOG_DEBUG("Init ADC_B -> OK");
    delay(1000);
#endif

    if (!spiA_ok || !spiB_ok)
    {
        LOG_ERROR("Unable to Initialize SPI Busses");
        LOG_ERROR("5 seconds to restart...");
        vTaskDelay(pdMS_TO_TICKS(5000));
        esp_restart();
    }

    LOG_DEBUG("Init SPI -> OK");

//////// INIT I2C INTERFACES ////////
#ifdef I2C_ENABLE
    LOG_DEBUG("Init I2C Interfaces");
    bool i2c_ok = true;
    i2c_ok = Wire.begin(SDA, SCL, 100000);
    if (!i2c_ok)
    {
        LOG_ERROR("Unable to Initialize I2C Bus");
        LOG_ERROR("5 seconds to restart...");
        vTaskDelay(pdMS_TO_TICKS(5000));
        esp_restart();
    }
    LOG_DEBUG("Init I2c ok");
    Serial.readStringUntil('\n');

    // Init IO Expanders
    dev->m_ext_io = std::make_unique<ExternalIO>(Wire, dev->m_i2c_mutex, EXPANDER_ALL_INTERRUPT);
#endif

//////// INIT REALTIME TASKS PARAMETERS ////////
#ifdef CH_A_RT_ENABLE
    const rtIgnitionTask::rtTaskParams taskA_params{
        .rt_running = true,
        .name = "rtIgnTask_A",
        .rt_stack_size = RT_TASK_STACK,
        .rt_priority = RT_TASK_PRIORITY,
        .rt_int = rtIgnitionTask::rtTaskInterruptParams{
            .isr_ptr = &trig_isr_A,
            .trig_pin_12p = TRIG_PIN_A12P,
            .trig_pin_12n = TRIG_PIN_A12N,
            .trig_pin_34p = TRIG_PIN_A34P,
            .trig_pin_34n = TRIG_PIN_A34N,
            .spark_pin_12 = SPARK_PIN_A12,
            .spark_pin_34 = SPARK_PIN_A34},
        .rt_io = rtIgnitionTask::rtTaskIOParams{
            .pot_cs_12 = POT_CS_A12,
            .pot_cs_34 = POT_CS_A34,
            .ss_force = SS_FORCE_A,
            .ss_inhibit_12 = SS_INIBHIT_A12,
            .ss_inhibit_34 = SS_INHIBIT_A34,
            .sh_disch_12 = SH_DISCH_A12,
            .sh_disch_34 = SH_DISCH_A34,
            .sh_arm_12 = SH_ARM_A12,
            .sh_arm_34 = SH_ARM_A34,
            .relay_in_12 = RELAY_IN_A12,
            .relay_in_34 = RELAY_OUT_A12,
            .relay_out_12 = RELAY_IN_A34,
            .relay_out_34 = RELAY_OUT_A34,
        },
        .rt_queue = nullptr,
        .dev = &dev};
#endif
#ifdef CH_B_RT_ENABLE
    const rtIgnitionTask::rtTaskParams taskB_params{
        .rt_running = true,
        .name = "rtIgnTask_B",
        .rt_stack_size = RT_TASK_STACK,
        .rt_priority = RT_TASK_PRIORITY,
        .rt_int = rtIgnitionTask::rtTaskInterruptParams{
            .isr_ptr = &trig_isr_B,
            .trig_pin_12p = TRIG_PIN_B12P,
            .trig_pin_12n = TRIG_PIN_B12N,
            .trig_pin_34p = TRIG_PIN_B34P,
            .trig_pin_34n = TRIG_PIN_B34N,
            .spark_pin_12 = SPARK_PIN_B12,
            .spark_pin_34 = SPARK_PIN_B34},
        .rt_io = rtIgnitionTask::rtTaskIOParams{
            .pot_cs_12 = POT_CS_B12,
            .pot_cs_34 = POT_CS_B34,
            .ss_force = SS_FORCE_B,
            .ss_inhibit_12 = SS_INIBHIT_B12,
            .ss_inhibit_34 = SS_INHIBIT_B34,
            .sh_disch_12 = SH_DISCH_B12,
            .sh_disch_34 = SH_DISCH_B34,
            .sh_arm_12 = SH_ARM_B12,
            .sh_arm_34 = SH_ARM_B34,
            .relay_in_12 = RELAY_IN_B12,
            .relay_in_34 = RELAY_OUT_B12,
            .relay_out_12 = RELAY_IN_B34,
            .relay_out_34 = RELAY_OUT_B34,
        },
        .rt_queue = nullptr,
        .dev = &dev};
#endif

    //////// SPAWN REALTIME TASKS ////////
    bool tasK_A_rt = true;
    bool task_B_rt = true;
    BaseType_t ignA_task_success = pdPASS;
    BaseType_t ignB_task_success = pdPASS;

#ifdef CH_A_RT_ENABLE
    auto task_A = rtIgnitionTask(taskA_params, PSRAM_MAX, QUEUE_MAX, CORE_1, fs_mutex);
    ignA_task_success = task_A.getStatus() == rtIgnitionTask::OK ? pdPASS : pdFAIL;
    //tasK_A_rt = task_A.start();
    delay(1000);
#endif

#ifdef CH_B_RT_ENABLE
    auto task_B = rtIgnitionTask(taskB_params, PSRAM_MAX, QUEUE_MAX, CORE_1, fs_mutex);
    ignB_task_success = task_B.getStatus() == rtIgnitionTask::OK ? pdPASS : pdFAIL;
    //task_B_rt = task_B.start();
    delay(1000);
#endif

    // Ignition A on Core 0
    if (ignA_task_success != pdPASS || ignB_task_success != pdPASS)
    {
        LOG_ERROR("Unable to initialize ISR task");
        LOG_ERROR("5 seconds to restart...");
        vTaskDelay(pdMS_TO_TICKS(5000));
        esp_restart();
    }
    if (tasK_A_rt != true || task_B_rt != true)
    {
        led.setStatus(RGBled::LedStatus::ERROR);
        LOG_ERROR("Unable to start realtime tasks");
    }
    else
    {
        LOG_DEBUG("Real Time Tasks A & B initialized");
        led.setStatus(RGBled::LedStatus::OK);
    }

    //////// SPAWN WEBSERVER and WEBSOCKET ////////
    ArduinoJson::JsonDocument json_data;
    bool data_a = false, data_b = false;
#ifdef WEB_ENABLE
    AstroWebServer webPage(80, LittleFS);
    delay(1000);
    task_A.onMessage([&webPage, &json_data, &data_a](ignitionBoxStatusFiltered sts)
                     {
    json_data["box_a"] = sts.toJson();
    data_a = true; });

    #ifdef CH_B_RT_ENABLE
    task_B.onMessage([&webPage, &json_data, &data_b](ignitionBoxStatusFiltered sts)
                     {
    json_data["box_b"] = sts.toJson();
    data_b = true; });
    #endif
#endif

    // task_A.enableSave(true, "ignitionA_test.csv");
    // task_B.enableSave(true, "ignitionB_test.csv");

    uint32_t monitor_loop = millis();
    uint32_t data_loop = monitor_loop;
    //////////////// INNER LOOP /////////////////////
    while (running)
    {
        uint32_t this_loop = millis();
        if (this_loop - monitor_loop > 5000)
        {
            clearScreen();
            printRunningTasksMod(Serial);
            monitor_loop = millis();
        }
        vTaskDelay(pdMS_TO_TICKS(10));
#ifdef WEB_ENABLE
        if ((data_a && data_b) || (this_loop - data_loop > 500))
        {
            webPage.sendWsData(json_data.as<String>());
            json_data.clear();
            data_a = data_b = false;
            data_loop = millis();
        }
#endif
    } //////////////// INNER LOOP /////////////////////

} ////////////////////// MAIN LOOP //////////////////////