#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>

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

// #define CH_B_ENABLE
#define TEST

// Debug Defines
#define WIFI_SSID "AstroRotaxMonitor"
#define WIFI_PASSWORD "maledettirotax"

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

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

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

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

void loop()
{
    // global variables
    bool running = true;
    const uint32_t max_queue = 128;
    const uint32_t filter_k = 10;
    PSRAMVector<ignitionBoxStatus> ignA_history_0(max_history);
    PSRAMVector<ignitionBoxStatus> ignA_history_1(max_history);
    auto *active_history = &ignA_history_0;
    auto *writable_history = &ignA_history_1;

    // Resources Initialization
    Devices dev;
    // Task handle
    TaskHandle_t trigA_TaskHandle = NULL;
    TaskHandle_t trigB_TaskHandle = NULL;
    // Data Queue for real time task to main loop communication
    QueueHandle_t rt_taskA_queue = xQueueCreate(max_queue, sizeof(ignitionBoxStatus));
    QueueHandle_t rt_taskB_queue = xQueueCreate(max_queue, sizeof(ignitionBoxStatus));
    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_peak= RST_EXT_PEAK_DETECT, .rst_io_sh = RST_EXT_SAMPLE_HOLD}};

    if (!rt_taskA_queue || !rt_taskB_queue)
    {
        LOG_ERROR("Unable To Create task queues");
        LOG_ERROR("5 seconds to restart...");
        vTaskDelay(pdMS_TO_TICKS(5000));
        esp_restart();
    }
    else
        LOG_DEBUG("Task Variables OK");

#ifdef CH_B_ENABLE
    QueueHandle_t rt_taskB_queue = xQueueCreate(max_queue, 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

    // Spi ok flags
    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
#ifdef CH_B_ENABLE
    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_DEBUG("Init SPI OK");

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

#ifdef CH_B_ENABLE
    // 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_DEBUG("Init ADC OK");

    // Ignition A on Core 0
    auto ignA_task_success = pdPASS;
    ignA_task_success = xTaskCreatePinnedToCore(
        rtIgnitionTask,
        "rtIgnitionTask_boxA",
        RT_TASK_STACK,
        (void *)&taskA_params,
        RT_TASK_PRIORITY,
        &trigA_TaskHandle,
        CORE_0);
    delay(100); // give some time to the thread to start

    // Ignition B on Core 1
    auto ignB_task_success = pdPASS;
#ifdef CH_B_ENABLE
    ignB_task_success = xTaskCreatePinnedToCore(
        rtIgnitionTask,
        "rtIgnitionTask_boxB",
        RT_TASK_STACK,
        (void *)&taskB_params,
        RT_TASK_PRIORITY, // priorità leggermente più alta
        &trigB_TaskHandle,
        CORE_1);
    delay(100); // give some time to the thread to start
#endif

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

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

    ////////////////////// MAIN LOOP //////////////////////
    bool partial_save = false; // flag to indicate if a partial save has been done after a timeout
    uint32_t counter = 0;
    uint32_t wait_count = 0;
    ignitionBoxStatus ign_info;
    ignitionBoxStatusAverage ign_info_avg(filter_k);
    LITTLEFSGuard fsGuard;
    WebPage webPage(80, LittleFS); // Initialize webserver and Websocket

    while (running)
    {
        if (counter >= active_history->size()) // not concurrent with write task
        {
            counter = 0;
            partial_save = false; // reset partial save flag on new data cycle
            auto *temp = active_history;
            active_history = writable_history; // switch active and writable buffers
            writable_history = temp;           // ensure writable_history points to the buffer we just filled
            dataSaveParams save_params{
                .history = writable_history,
                .file_path = "ignition_history.csv"};
            save_history(*writable_history, "ignition_history.csv"); // directly call the save task function to save without delay
        }

        if (xQueueReceive(rt_taskA_queue, &ign_info, pdMS_TO_TICKS(1000)) == pdTRUE)
        {
            // printInfo(ign_info);
            auto &hist = *active_history;
            hist[counter++ % active_history->size()] = ign_info;
            ign_info_avg.update(ign_info); // update moving average with latest ignition status
            Serial.printf("\033[2K Data Received: %d/%d\r", counter, hist.size());
            if ( counter % filter_k == 0) // send data every 10 samples
            {
                Serial.println();
                LOG_DEBUG("Sending average ignition status to websocket clients...");
                webPage.sendWsData(ign_info_avg.toJson().as<String>());
            }
        }
        else
        {
            Serial.printf("[%d] Waiting for data...\r", wait_count++);
            if (!partial_save && counter > 0) // if timeout occurs but we have unsaved data, save it before next timeout
            {
                active_history->resize(counter); // resize active history to actual number of records received to avoid saving empty records
                save_history(*active_history, "ignition_history.csv");
                active_history->resize(max_history); // resize back to max history size for next data cycle
                counter = 0;                         // reset counter after saving
                partial_save = true;
                first_save = true;
            }
            delay(500);
        }
    }

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