#define DEBUGLOG_DEFAULT_LOG_LEVEL_INFO

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

// Definitions
#include <pins.h>
#include <channels.h>
#include <tasks.h>

// Device Libraries
#include <ADS1256.h>
#include <AD5292.h>

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

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

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

  // Initialize Interrupt pins on coil detectors
  pinMode(TRIG_A12P, INPUT_PULLDOWN);
  pinMode(TRIG_A12N, INPUT_PULLDOWN);
  pinMode(TRIG_A34P, INPUT_PULLDOWN);
  pinMode(TRIG_A34N, INPUT_PULLDOWN);
  pinMode(TRIG_B12P, INPUT_PULLDOWN);
  pinMode(TRIG_B12N, INPUT_PULLDOWN);
  pinMode(TRIG_B34P, INPUT_PULLDOWN);
  pinMode(TRIG_B34N, INPUT_PULLDOWN);
  initTriggerPinMapping();
  
  // Initialize Interrupt pins on spark detectors
  pinMode(SPARK_A12, INPUT_PULLDOWN);
  pinMode(SPARK_A34, INPUT_PULLDOWN);
  pinMode(SPARK_B12, INPUT_PULLDOWN);
  pinMode(SPARK_B34, INPUT_PULLDOWN);
  initSparkPinMapping();

  // Ignition A Interrupts 
  attachInterrupt(TRIG_A12P, trig_isr_a, RISING);
  attachInterrupt(TRIG_A34P, trig_isr_a, RISING);
  attachInterrupt(TRIG_A12N, trig_isr_a, RISING);
  attachInterrupt(TRIG_A34N, trig_isr_a, RISING);
  attachInterrupt(SPARK_A12, spark_a, RISING);
  attachInterrupt(SPARK_A34, spark_a, RISING);
  // Ignition B Interrupts
  attachInterrupt(TRIG_B12P, trig_isr_b, RISING);
  attachInterrupt(TRIG_B34P, trig_isr_b, RISING);
  attachInterrupt(TRIG_B12N, trig_isr_b, RISING);
  attachInterrupt(TRIG_B34N, trig_isr_b, RISING);
  attachInterrupt(SPARK_B12, spark_b, RISING);
  attachInterrupt(SPARK_B34, spark_b, RISING);

  // Init SPI interface
  SPI.begin();
}

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

  // Ignition A on Core 0
    auto ignA_task_success = xTaskCreatePinnedToCore(
        ignitionA_task,
        "ignitionA_task",
        TASK_STACK,
        NULL,
        TASK_PRIORITY, 
        &trigA_TaskHandle,
        CORE_0
    );

    // Ignition A on Core 1
    auto ignB_task_success = xTaskCreatePinnedToCore(
        ignitionB_task,
        "ignitionB_task",
        TASK_STACK,
        NULL,
        TASK_PRIORITY, // priorità leggermente più alta
        &trigA_TaskHandle,
        CORE_1
    );

    if ((ignA_task_success && ignB_task_success) != pdPASS){
      LOG_ERROR("Unble to initialize ISR task");
    }
    LOG_INFO("Real Time Tasks A&B initialized");


    ////////////////////// MAIN LOOP //////////////////////
    while (running) {

    }

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