#include "tasks.h"
#include <esp_timer.h>

// Timeout callback for microsecond precision
void spark_timeout_callback(void *arg)
{
    TaskHandle_t handle = (TaskHandle_t)arg;
    xTaskNotify(handle, SPARK_FLAG_TIMEOUT, eSetValueWithOverwrite);
}

void rtIgnitionTask(void *pvParameters)
{

    // Invalid real time rt_task_ptr parameters, exit immediate
    if (!pvParameters)
    {
        LOG_ERROR("Null rt_task_ptr parameters");
        vTaskDelete(NULL);
    }
    LOG_INFO("rtTask Params OK");

    // Task Parameters and Devices
    rtTaskParams *params = (rtTaskParams *)pvParameters;
    const rtTaskInterrupts rt_int = params->rt_int; // copy to avoid external override
    const rtTaskResets rt_rst = params->rt_resets;  // copy to avoid external override
    QueueHandle_t rt_queue = params->rt_queue;
    TaskHandle_t rt_handle_ptr = *params->rt_handle_ptr;
    Devices *dev = params->dev;
    ADS1256 *adc = dev->adc_a;
    PCA9555 *io = dev->io;

    ignitionBoxStatus ign_box_sts;

    // Variables for ISR, static to be fixed in memory locations
    static isrParams isr_params_t12p{
        .flag = TRIG_FLAG_12P,
        .ign_stat = &ign_box_sts,
        .rt_handle_ptr = rt_handle_ptr};
    static isrParams isr_params_t12n{
        .flag = TRIG_FLAG_12N,
        .ign_stat = &ign_box_sts,
        .rt_handle_ptr = rt_handle_ptr};
    static isrParams isr_params_t34p{
        .flag = TRIG_FLAG_34P,
        .ign_stat = &ign_box_sts,
        .rt_handle_ptr = rt_handle_ptr};
    static isrParams isr_params_t34n{
        .flag = TRIG_FLAG_34N,
        .ign_stat = &ign_box_sts,
        .rt_handle_ptr = rt_handle_ptr};
    static isrParams isr_params_sp12{
        .flag = SPARK_FLAG_12,
        .ign_stat = &ign_box_sts,
        .rt_handle_ptr = rt_handle_ptr};
    static isrParams isr_params_sp34{
        .flag = SPARK_FLAG_34,
        .ign_stat = &ign_box_sts,
        .rt_handle_ptr = rt_handle_ptr};

    LOG_INFO("rtTask ISR Params OK");

    // Create esp_timer for microsecond precision timeout
    esp_timer_handle_t timeout_timer;
    esp_timer_create_args_t timer_args = {
        .callback = spark_timeout_callback,
        .arg = (void *)rt_handle_ptr,
        .dispatch_method = ESP_TIMER_TASK,
        .name = "spark_timeout"};
    esp_timer_create(&timer_args, &timeout_timer);

    // Attach Pin Interrupts
    attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_12p), rt_int.isr_ptr, (void *)&isr_params_t12p, RISING);
    attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_12n), rt_int.isr_ptr, (void *)&isr_params_t12n, RISING);
    attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_34p), rt_int.isr_ptr, (void *)&isr_params_t34p, RISING);
    attachInterruptArg(digitalPinToInterrupt(rt_int.trig_pin_34n), rt_int.isr_ptr, (void *)&isr_params_t34n, RISING);
    attachInterruptArg(digitalPinToInterrupt(rt_int.spark_pin_12), rt_int.isr_ptr, (void *)&isr_params_sp12, RISING);
    attachInterruptArg(digitalPinToInterrupt(rt_int.spark_pin_34), rt_int.isr_ptr, (void *)&isr_params_sp34, RISING);

    LOG_INFO("rtTask ISR Attach OK");

    // Compute Reset Pin Bitmask
    const uint16_t rst_bitmask = (1 << rt_rst.rst_io_12p) |
                                 (1 << rt_rst.rst_io_12n) |
                                 (1 << rt_rst.rst_io_34p) |
                                 (1 << rt_rst.rst_io_34n);

    LOG_WARN("rtTask Init Correct");
    // Global rt_task_ptr variables
    bool first_cycle = true;
    bool cycle12 = false;
    bool cycle34 = false;
    int64_t last_cycle_time = 0;
    uint32_t n_errors = 0;

    while (params->rt_running)
    {
        uint32_t pickup_flag = 0;
        uint32_t spark_flag = 0;

        // WAIT FOR PICKUP SIGNAL
        xTaskNotifyWait(
            0x00,         // non pulire all'ingresso
            ULONG_MAX,    // pulisci i primi 8 bit
            &pickup_flag, // valore ricevuto
            portMAX_DELAY);

        if (first_cycle && pickup_flag != TRIG_FLAG_12P) // skip first cycle because of possible initial noise on pickup signals at startu
            continue;

#ifdef DEBUG
        Serial.print("\033[2J"); // clear screen
        Serial.print("\033[H");  // cursor home

        if (!names.contains(pickup_flag))
        {
            LOG_ERROR("Wrong Pickup Flag");
            LOG_ERROR("Pickup Flags: ", printBits(pickup_flag).c_str());
            continue;
        }
        else
        {
            LOG_INFO("Pickup Trigger: ", names.at(pickup_flag));
        }
#endif

        // Start microsecond precision timeout timer
        esp_timer_stop(timeout_timer); // stop timer in case it was running from previous cycle
        esp_timer_start_once(timeout_timer, spark_timeout_max);

        // WAIT FOR SPARK TO HAPPEN OR TIMEOUT
        xTaskNotifyWait(
            0x00,           // non pulire all'ingresso
            ULONG_MAX,      // pulisci i primi 8 bit
            &spark_flag,    // valore ricevuto
            portMAX_DELAY); // wait indefinitely, timeout handled by esp_timer

        // Handle timeout or spark event
        if (spark_flag != SPARK_FLAG_TIMEOUT)
            esp_timer_stop(timeout_timer);

        // A trigger from pickup 12 is followed by a spark event on 34 or vice versa pickup 34 triggers spark on 12
        if ((pickup_flag == TRIG_FLAG_12P || pickup_flag == TRIG_FLAG_12N) && (spark_flag != SPARK_FLAG_12 && spark_flag != SPARK_FLAG_TIMEOUT))
        {
            ign_box_sts.coils12.spark_status = ign_box_sts.coils34.spark_status = sparkStatus::SPARK_SYNC_FAIL;
            continue;
        }

        // Select coil status reference based on pickup_flag
        coilsStatus *coils;
        switch (pickup_flag)
        {
        case TRIG_FLAG_12P:
        {
            first_cycle = false;
            // compute engine rpm from cycle time
            auto current_time = esp_timer_get_time();
            auto cycle_time = current_time - last_cycle_time;
            last_cycle_time = current_time;
            ign_box_sts.eng_rpm = (int32_t)(60.0f / (cycle_time / 1000000.0f));
        }
        case TRIG_FLAG_12N:
            coils = &ign_box_sts.coils12;
            break;
        case TRIG_FLAG_34P:
        case TRIG_FLAG_34N:
            coils = &ign_box_sts.coils34;
            break;
        }

        // Select logic based on pickup and spark flags
        switch (pickup_flag)
        {
        case TRIG_FLAG_12P:
        case TRIG_FLAG_34P:
        {
            // Timeout not occourred, expected POSITIVE edge spark OCCOURRED
            if (spark_flag != SPARK_FLAG_TIMEOUT)
            {
                coils->spark_delay = (int32_t)(coils->spark_time - coils->trig_time);
                coils->sstart_status = softStartStatus::NORMAL;  // because spark on positive edge
                coils->spark_status = sparkStatus::SPARK_POS_OK; // do not wait for spark on negative edge
            }
            // Timeout occourred, expected POSITIVE edge spark NOT OCCOURRED
            else if (spark_flag == SPARK_FLAG_TIMEOUT)
            {
                coils->spark_status = sparkStatus::SPARK_NEG_WAIT;
                coils->sstart_status = softStartStatus::NORMAL;
            }
            continue; // Do nothing more on positive pulse
        }
        // CASES for NEGATIVE cycle triggering of pickup and sparks 12 & 34
        case TRIG_FLAG_12N:
        case TRIG_FLAG_34N:
        {
            const bool expected_negative = coils->spark_status == sparkStatus::SPARK_NEG_WAIT;
            // Timeout not occourred, expected NEGATIVE edge spark OCCOURRED
            if (spark_flag != SPARK_FLAG_TIMEOUT && expected_negative)
            {
                coils->spark_delay = (int32_t)(coils->spark_time - coils->trig_time);
                coils->sstart_status = softStartStatus::SOFT_START;
                coils->spark_status = sparkStatus::SPARK_NEG_OK;
            }
            // Timeout occourred, expected POSITIVE edge spark NOT OCCOURRED
            else if (spark_flag == SPARK_FLAG_TIMEOUT && expected_negative)
            {
                coils->sstart_status = softStartStatus::ERROR;
                coils->spark_status = sparkStatus::SPARK_NEG_FAIL;
            }
            // Timeout not occouured, unexpected negative edge spark
            else if (spark_flag != SPARK_FLAG_TIMEOUT && !expected_negative)
            {
                coils->sstart_status = softStartStatus::SOFT_START;
                coils->spark_status = sparkStatus::SPARK_NEG_UNEXPECTED;
            }
            // Wait for finish of negative pulse to save data to buffer
            coils->n_events++;
            if (pickup_flag == TRIG_FLAG_12N)
                cycle12 = true;
            else
                cycle34 = true;
            break;
        }
        default:
            break;
        }

        if (cycle12 && cycle34) // wait for both 12 and 34 cycles to complete before sending data to main loop and resetting peak detectors
        {
            cycle12 = false;
            cycle34 = false;

            if (ign_box_sts.coils12.spark_status == sparkStatus::SPARK_POS_FAIL || ign_box_sts.coils12.spark_status == sparkStatus::SPARK_NEG_FAIL)
                ign_box_sts.coils12.n_missed_firing++;
            if (ign_box_sts.coils34.spark_status == sparkStatus::SPARK_POS_FAIL || ign_box_sts.coils34.spark_status == sparkStatus::SPARK_NEG_FAIL)
                ign_box_sts.coils34.n_missed_firing++;

            //  read adc channels: pickup12, out12 [ pos + neg ]
            if (adc) // read only if adc initialized
            {
                uint32_t start_adc_read = esp_timer_get_time();
                // from peak detector circuits
                ign_box_sts.coils12.peak_p_in = adcReadChannel(adc, ADC_CH_PEAK_12P_IN);
                ign_box_sts.coils12.peak_n_in = adcReadChannel(adc, ADC_CH_PEAK_12N_IN);
                ign_box_sts.coils34.peak_p_in = adcReadChannel(adc, ADC_CH_PEAK_34P_IN);
                ign_box_sts.coils34.peak_n_in = adcReadChannel(adc, ADC_CH_PEAK_34N_IN);
                ign_box_sts.coils12.peak_p_out = adcReadChannel(adc, ADC_CH_PEAK_12P_OUT);
                ign_box_sts.coils12.peak_n_out = adcReadChannel(adc, ADC_CH_PEAK_12N_OUT);
                ign_box_sts.coils34.peak_p_out = adcReadChannel(adc, ADC_CH_PEAK_34P_OUT);
                ign_box_sts.coils34.peak_n_out = adcReadChannel(adc, ADC_CH_PEAK_34N_OUT);
                ign_box_sts.adc_read_time = (int32_t)(esp_timer_get_time() - start_adc_read);
            }
            else // simulate adc read timig
                vTaskDelay(pdMS_TO_TICKS(1));

            // reset peak detectors + sample and hold
            // outputs on io expander
            if (io)
            {
                const uint16_t iostat = io->read();
                io->write(iostat | rst_bitmask);
                vTaskDelay(pdMS_TO_TICKS(1));
                io->write(iostat & ~rst_bitmask);
            }
            else
                vTaskDelay(pdMS_TO_TICKS(1));

            // send essage to main loop with ignition info, by copy so local static variable is ok
            if (rt_queue)
            {
                ign_box_sts.timestamp = esp_timer_get_time(); // update data timestamp
                if (xQueueSendToBack(rt_queue, (void *)&ign_box_sts, 0) != pdPASS)
                    ign_box_sts.n_queue_errors = ++n_errors;
            }
        }
    }
    // Delete the timeout timer
    esp_timer_delete(timeout_timer);
    LOG_WARN("Ending realTime Task");
    // Ignition A Interrupts DETACH
    detachInterrupt(rt_int.trig_pin_12p);
    detachInterrupt(rt_int.trig_pin_12n);
    detachInterrupt(rt_int.trig_pin_34p);
    detachInterrupt(rt_int.trig_pin_34n);
    detachInterrupt(rt_int.spark_pin_12);
    detachInterrupt(rt_int.spark_pin_34);
    // delete present task
    vTaskDelete(NULL);
}
