#define DEBUGLOG_DEFAULT_LOG_LEVEL_INFO

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

// Definitions
#include <tasks.h>
#include <devices.h>
#include <ui.h>

// FreeRTOS directives
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

// #define CH_B_ENABLE
#define TEST

void printTaskStats()
{
    char buffer[1024];
    vTaskGetRunTimeStats(buffer);
    Serial.println(buffer);
}

void setup()
{
    Serial.begin(921600);
    delay(250);

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

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

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

void loop()
{
    // global variables
    bool running = true;
    static Devices dev;

    // Task handle
    static TaskHandle_t trigA_TaskHandle = NULL;
    static TaskHandle_t trigB_TaskHandle = NULL;

    static QueueHandle_t rt_taskA_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
    static QueueHandle_t rt_taskB_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
    static rtTaskParams taskA_params{
        .rt_running = true,
        .dev = &dev,
        .rt_handle_ptr = &trigA_TaskHandle,
        .rt_queue = rt_taskA_queue,
        .rt_int = rtTaskInterrupts{
            .isr_ptr = trig_isr,
            .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_resets = rtTaskResets{.rst_io_12p = RST_EXT_A12P, .rst_io_12n = RST_EXT_A12N, .rst_io_34p = RST_EXT_A34P, .rst_io_34n = RST_EXT_A34N}};

    LOG_INFO("Task Variables OK");

#ifdef CH_B_ENABLE
    QueueHandle_t rt_taskB_queue = xQueueCreate(10, sizeof(ignitionBoxStatus));
    rtTaskParams taskB_params{
        .rt_running = true,
        .dev = &dev,
        .rt_handle_ptr = &trigB_TaskHandle,
        .rt_queue = rt_taskB_queue,
        .rt_int = rtTaskInterrupts{
            .isr_ptr = trig_isr,
            .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_resets = rtTaskResets{.rst_io_12p = RST_EXT_B12P, .rst_io_12n = RST_EXT_B12N, .rst_io_34p = RST_EXT_B34P, .rst_io_34n = RST_EXT_B34N}};
#endif

    bool spiA_ok = true;
    bool spiB_ok = true;

    // Init 2 SPI interfaces
    SPIClass SPI_A(FSPI);
    spiA_ok = SPI_A.begin(SPI_A_SCK, SPI_A_MISO, SPI_A_MOSI);
    SPI_A.setDataMode(SPI_MODE1); // ADS1256 requires SPI mode 1
    #ifndef TEST
    SPIClass SPI_B(HSPI);
    spiB_ok = SPI_B.begin(SPI_B_SCK, SPI_B_MISO, SPI_B_MOSI);
    SPI_B.setDataMode(SPI_MODE1); // ADS1256 requires SPI mode 1
    #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_INFO("Init SPI OK");

    // Init ADC_A
    dev.adc_a = new ADS1256(ADC_A_DRDY, ADS1256::PIN_UNUSED, ADC_A_SYNC, ADC_A_CS, 2.5, &SPI_A);
    dev.adc_a->InitializeADC();
    dev.adc_a->setPGA(PGA_1);
    dev.adc_a->setDRATE(DRATE_7500SPS);

#ifndef TEST
    // Init ADC_B
    dev.adc_a = new ADS1256(ADC_B_DRDY, ADC_B_RST, ADC_B_SYNC, ADC_B_CS, 2.5, &SPI_B);
    dev.adc_a->InitializeADC();
    dev.adc_a->setPGA(PGA_1);
    dev.adc_a->setDRATE(DRATE_1000SPS);
#endif

    LOG_INFO("Init ADC OK");

    // Ignition A on Core 0
    auto ignA_task_success = pdPASS;
    ignA_task_success = xTaskCreatePinnedToCore(
        rtIgnitionTask,
        "rtIgnitionTask_boxA",
        TASK_STACK,
        (void *)&taskA_params,
        TASK_PRIORITY,
        &trigA_TaskHandle,
        CORE_0);

    // Ignition B on Core 1
    auto ignB_task_success = pdPASS;
#ifdef CH_B_ENABLE
    ignB_task_success = xTaskCreatePinnedToCore(
        rtIgnitionTask,
        "rtIgnitionTask_boxB",
        TASK_STACK,
        (void *)&taskB_params,
        TASK_PRIORITY, // priorità leggermente più alta
        &trigB_TaskHandle,
        CORE_1);
#endif

    if ((ignA_task_success && ignB_task_success) != pdPASS)
    {
        LOG_ERROR("Unble to initialize ISR task");
        LOG_ERROR("5 seconds to restart...");
        vTaskDelay(pdMS_TO_TICKS(5000));
        esp_restart();
    }

    LOG_INFO("Real Time Tasks A & B initialized");

    ////////////////////// MAIN LOOP //////////////////////
    clearScreen();
    setCursor(0, 0);
    ignitionBoxStatus ignA;
    int64_t last = esp_timer_get_time();
    uint32_t missed_firings12 = 0;
    uint32_t missed_firings34 = 0;
    uint32_t counter = 0;

    while (running)
    {
        if (xQueueReceive(rt_taskA_queue, &ignA, pdMS_TO_TICKS(1000)) == pdTRUE)
        {
            if (ignA.coils12.spark_status == sparkStatus::SPARK_POS_FAIL || ignA.coils12.spark_status == sparkStatus::SPARK_NEG_FAIL)
                missed_firings12++;
            if (ignA.coils34.spark_status == sparkStatus::SPARK_POS_FAIL || ignA.coils34.spark_status == sparkStatus::SPARK_NEG_FAIL)
                missed_firings34++;

            clearScreen();
            setCursor(0, 0);
            printField("++ Timestamp", (uint32_t)ignA.timestamp);
            Serial.println("========== Coils 12 =============");
            printField("Events", ignA.coils12.n_events);
            printField("Missed Firing", missed_firings12);
            printField("Spark Dly", (uint32_t)ignA.coils12.spark_delay);
            printField("Spark Sts", sparkStatusNames.at(ignA.coils12.spark_status));
            printField("Peak P_IN", ignA.coils12.peak_p_in);
            printField("Peak N_IN", ignA.coils12.peak_n_in);
            printField("Peak P_OUT", ignA.coils12.peak_p_out);
            printField("Peak N_OUT", ignA.coils12.peak_n_out);
            printField("Soft Start ", softStartStatusNames.at(ignA.coils12.sstart_status));

            Serial.println("========== Coils 34 =============");
            printField("Events", ignA.coils34.n_events);
            printField("Missed Firing", missed_firings34);
            printField("Spark Dly", (uint32_t)ignA.coils34.spark_delay);
            printField("Spark Sts", sparkStatusNames.at(ignA.coils34.spark_status));
            printField("Peak P_IN", ignA.coils34.peak_p_in);
            printField("Peak N_IN", ignA.coils34.peak_n_in);
            printField("Peak P_OUT", ignA.coils34.peak_p_out);
            printField("Peak N_OUT", ignA.coils34.peak_n_out);
            printField("Soft Start ", softStartStatusNames.at(ignA.coils34.sstart_status));

            Serial.println("========== END =============");
            Serial.println();
            printField("Engine RPM", ignA.eng_rpm);
            printField("ADC Read Time", ignA.adc_read_time);
            printField("Queue Errors", ignA.n_queue_errors);
        }
        else
        {
            Serial.println("Waiting for data... ");
            delay(500);
        }
    }

    if (trigA_TaskHandle)
        vTaskDelete(trigA_TaskHandle);
    if (trigB_TaskHandle)
        vTaskDelete(trigB_TaskHandle);
    ////////////////////// MAIN LOOP //////////////////////
}