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

//// GLOBAL STATIC FUNCTIONS

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

// Manages queue receive, save data and callback to external tasks for communication
void rtIgnitionTask::rtIgnitionTask_manager(void *pvParameters)
{
    rtIgnitionTask *cls = (rtIgnitionTask *)pvParameters;
    auto last_loop = millis();
    uint32_t count(0);
    while (cls->m_running)
    {
        cls->run();
        vTaskDelay(pdMS_TO_TICKS(1));
    }
}

// Static task function
void rtIgnitionTask::rtIgnitionTask_realtime(void *pvParameters)
{

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

    // Task Parameters and Devices
    rtTaskParams *params = (rtTaskParams *)pvParameters;
    const rtTaskInterruptParams rt_int = params->rt_int; // copy to avoid external override
    const rtTaskIOParams rt_rst = params->rt_io;         // copy to avoid external override
    QueueHandle_t rt_queue = params->rt_queue;
    Devices *dev = params->dev;
    ExternalIO *io = dev->m_ext_io;
    ADS1256 *adc = params->name == "rtIgnTask_A" ? dev->m_adc_a : dev->m_adc_b;
    std::mutex &spi_mutex = params->name == "rtIgnTask_A" ? dev->m_spi_a_mutex : dev->m_spi_b_mutex;

    TaskStatus_t rt_task_info;
    vTaskGetInfo(NULL, &rt_task_info, pdFALSE, eInvalid);

    LOG_INFO("rtTask Params OK [", params->name.c_str(), "]");

    ignitionBoxStatus ign_box_sts;

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

    // 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_task_info.xHandle,
        .dispatch_method = ESP_TIMER_TASK,
        .name = "spark_timeout"};
    if (esp_timer_create(&timer_args, &timeout_timer) != ESP_OK)
    {
        LOG_INFO("rtTask [", params->name.c_str(), "] Fail to allocate timeoutTimer");
        vTaskDelete(NULL);
    }

    // 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 [", params->name.c_str(), "]");

    // 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;

        // 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
        {
            // disable interrupts during adc samples
            disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12p));
            disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12n));
            disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34p));
            disableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34n));
            disableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_12));
            disableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_34));

            // reset coils 12 and 34 cycles
            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
            {
                std::lock_guard<std::mutex> lock(spi_mutex);
                uint32_t start_adc_read = esp_timer_get_time();
                // from peak detector circuits
                ign_box_sts.coils12.peak_p_in = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.coils12.peak_n_in = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.coils34.peak_p_in = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.coils34.peak_n_in = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.coils12.peak_p_out = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.coils12.peak_n_out = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.coils34.peak_p_out = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.coils34.peak_n_out = adc->convertToVoltage(adc->cycleSingle());
                ign_box_sts.adc_read_time = (int32_t)(esp_timer_get_time() - start_adc_read);
                adc->stopConversion();
            }
            else // simulate adc read timig
                vTaskDelay(pdMS_TO_TICKS(c_adc_time));

            // reset peak detectors + sample and hold
            // outputs on io expander
            if (io)
            {
                // Discharge Pulse
                io->extDigitalWrite(rt_rst.sh_disch_12, true);
                io->extDigitalWrite(rt_rst.sh_disch_34, true);
                delayMicroseconds(250);
                io->extDigitalWrite(rt_rst.sh_disch_12, false);
                io->extDigitalWrite(rt_rst.sh_disch_34, false);
                // Safety delay
                delayMicroseconds(500);
                // Re-Arm Pulse
                io->extDigitalWrite(rt_rst.sh_arm_12, true);
                io->extDigitalWrite(rt_rst.sh_arm_34, true);
                delayMicroseconds(250);
                io->extDigitalWrite(rt_rst.sh_arm_12, false);
                io->extDigitalWrite(rt_rst.sh_arm_34, false);
            }
            else
                vTaskDelay(pdMS_TO_TICKS(c_io_time));

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

            // enable interrupts ready for a new cycle
            enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12p));
            enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_12n));
            enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34p));
            enableInterrupt(digitalPinToInterrupt(rt_int.trig_pin_34n));
            enableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_12));
            enableInterrupt(digitalPinToInterrupt(rt_int.spark_pin_34));
        }
    }
    // Delete the timeout timer
    esp_timer_stop(timeout_timer);
    esp_timer_delete(timeout_timer);
    LOG_WARN("rtTask Ending [", params->name.c_str(), "]");
    // 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);
}

///////////// CLASS MEMBER DEFINITIONS /////////////
rtIgnitionTask::rtIgnitionTask(const rtTaskParams params, const uint32_t history_size, const uint32_t queue_size, const uint8_t core, std::mutex &fs_mutex, fs::FS &filesystem) : m_params(params), m_filesystem(filesystem), m_filesystemMutex(fs_mutex), m_core(core), m_historyMax(history_size)
{
    LOG_WARN("Starting Manager for [", m_params.name.c_str(), "]");
    // create queue buffers
    m_rtQueueHandle = xQueueCreate(queue_size, sizeof(ignitionBoxStatus));
    if (!m_rtQueueHandle)
    {
        LOG_ERROR("Unable To Create Task [", params.name.c_str(), "] queues");
        m_managerStatus = rtTaskStatus::ERROR;
        return;
    }
    else
        m_params.rt_queue = m_rtQueueHandle;

    try
    {
        // create PSram history vectors
        m_historyBuf0 = PSHistory(history_size);
        m_historyBuf1 = PSHistory(history_size);
        // assing active and writable history
        m_historyActive = std::unique_ptr<PSHistory>(&m_historyBuf0);
        m_historyInactive = std::unique_ptr<PSHistory>(&m_historyBuf1);
    }
    catch (std::bad_alloc &e)
    {
        LOG_ERROR("Task [", params.name.c_str(), "] Unable to allocate history PSRAM: ", e.what());
        return;
    }

    m_managerTaskName = (std::string("man_") + m_params.name).c_str();
    auto task_success = xTaskCreatePinnedToCore(
        rtIgnitionTask_manager,
        m_managerTaskName.c_str(),
        RT_TASK_STACK,
        (void *)this,
        m_params.rt_priority >> 2,
        &m_managerHandle,
        m_core);

    if (task_success != pdPASS)
    {
        LOG_ERROR("Unable To Create Manager for [", params.name.c_str(), "]");
        m_managerStatus = rtTaskStatus::ERROR;
        return;
    }

    // average every 10 samples
    m_statusFiltered = ignitionBoxStatusFiltered(m_filterSize);
    m_dataLast = millis();
    m_managerStatus = rtTaskStatus::OK;
}

rtIgnitionTask::~rtIgnitionTask()
{
    if (m_rtHandle)
        vTaskDelete(m_rtHandle);
    if (m_managerHandle)
        vTaskDelete(m_managerHandle);
    if (m_rtQueueHandle)
        vQueueDelete(m_rtQueueHandle);
}

void rtIgnitionTask::run()
{
    // receive new data from the queue
    auto new_data = xQueueReceive(m_rtQueueHandle, &m_statusLast, 0); // non blocking receive

    if (new_data == pdPASS)
    {
        m_dataLast = millis();
        m_managerStatus = rtTaskStatus::RUNNING;
        // if history buffer is full swap buffers and if enabled save history buffer
        if (m_statusCounter >= m_historyMax)
        {
            LOG_DEBUG("Save for Buffer Full: ", m_statusCounter);
            m_statusCounter = 0;
            m_savePartial = false; // reset partial save flag on new data cycle
            std::swap(m_historyActive, m_historyInactive);
            if (m_historySaveEnable)
                saveHistory(*m_historyInactive, m_historyPath); // directly call the save task function to save without delay
                LOG_INFO("Save History");
        }

        // update filtered data
        m_statusFiltered.update(m_statusLast);
        (*m_historyActive)[m_statusCounter] = m_statusLast;

        // callback 
        if (m_onFilteredStatusUpdate && m_statusCounter % m_filterSize == 0)
        {
            m_onFilteredStatusUpdate(m_statusFiltered);
        }

        // update data counter
        m_statusCounter++;
    }
    else
    {
        if (millis() - m_dataLast > c_idle_time)
        {
            if (m_statusCounter > 0 && !m_savePartial)
            {
                LOG_DEBUG("Save Partial: ", m_statusCounter);
                m_historyActive->resize(m_statusCounter);
                saveHistory(*m_historyActive, m_historyPath);
                m_historyActive->resize(m_historyMax);
                m_statusCounter = 0;
                m_savePartial = true;
            }
            m_managerStatus = rtTaskStatus::IDLE;
        }
    }
}

const bool rtIgnitionTask::start()
{
    LOG_WARN("Starting rtTask [", m_params.name.c_str(), "]");
    auto task_success = xTaskCreatePinnedToCore(
        rtIgnitionTask_realtime,
        m_params.name.c_str(),
        m_params.rt_stack_size,
        (void *)&m_params,
        m_params.rt_priority,
        &m_rtHandle,
        m_core);
    const bool success = task_success == pdPASS && m_rtHandle != nullptr;
    if (success)
        m_managerStatus = rtTaskStatus::IDLE;
    return success;
}

const bool rtIgnitionTask::stop()
{
    LOG_WARN("Ending Task [", m_params.name.c_str(), "]");
    if (m_rtHandle)
    {
        m_params.rt_running = false;
        m_rtHandle = nullptr;
        m_managerStatus = rtTaskStatus::STOPPED;
        return true;
    }
    return false;
}

const ignitionBoxStatus rtIgnitionTask::getLast() const
{
    return m_statusLast;
}

const ignitionBoxStatusFiltered rtIgnitionTask::getFiltered() const
{
    return m_statusFiltered;
}

const rtIgnitionTask::rtTaskStatus rtIgnitionTask::getStatus() const
{
    return m_managerStatus;
}

void rtIgnitionTask::enableSave(const bool enable, const std::filesystem::path filename)
{
    m_historySaveEnable = enable;
    if (enable && !filename.empty())
    {
        LOG_WARN("Save History Enabled Task [", m_params.name.c_str(), "]");
        m_historyPath = m_filesystem.mountpoint() / filename;
    }
    else
    {
        LOG_WARN("Save History Disabled Task [", m_params.name.c_str(), "]");
    }
}

void rtIgnitionTask::onMessage(std::function<void(ignitionBoxStatusFiltered)> callaback)
{
    m_onFilteredStatusUpdate = callaback;
}

void rtIgnitionTask::saveHistory(const rtIgnitionTask::PSHistory &history, const std::filesystem::path &file_name)
{
    // Lock filesystem mutex to avoid concurrent access
    std::lock_guard<std::mutex> fs_lock(m_filesystemMutex);

    // Check for free space
    if (LittleFS.totalBytes() - LittleFS.usedBytes() < history.size() * sizeof(ignitionBoxStatus)) // check if at least 1MB is free for saving history
    {
        LOG_ERROR("Not enough space in SPIFFS to save history");
        return;
    }

    // create complete file path
    const std::filesystem::path mount_point = std::filesystem::path(m_filesystem.mountpoint());
    std::filesystem::path file_path = file_name;
    if (file_name.root_path() != mount_point)
        file_path = mount_point / file_name;

    // if firt save remove old file and create new
    auto save_flags = std::ios::out;
    if (m_saveFirst)
    {
        save_flags |= std::ios::trunc;          // overwrite existing file
        m_filesystem.remove(file_path.c_str()); // ensure file is removed before saving to avoid issues with appending to existing file in SPIFFS
        LOG_INFO("Saving history to Flash, new file:", file_path.c_str());
    }
    else // else append to existing file
    {
        save_flags |= std::ios::app; // append to new file
        LOG_INFO("Saving history to Flash, appending to existing file:", file_path.c_str());
    }

    std::ofstream ofs(file_path, save_flags);
    if (ofs.fail())
    {
        LOG_ERROR("Failed to open file for writing");
        return;
    }

    // write csv header
    if (m_saveFirst)
    {
        ofs << "TS,EVENTS_12,DLY_12,STAT_12,V_12_1,V_12_2,V_12_3,V_12_4,IGNITION_MODE_12,"
            << "EVENTS_34,DLY_34,STAT_34,V_34_1,V_34_2,V_34_3,V_34_4,IGNITION_MODE_34,"
            << "ENGINE_RPM,ADC_READTIME,N_QUEUE_ERRORS"
            << std::endl;
        ofs.flush();
        m_saveFirst = false;
    }

    for (const auto &entry : history)
    {
        ofs << std::to_string(entry.timestamp) << ","
            << std::to_string(entry.coils12.n_events) << ","
            << std::to_string(entry.coils12.spark_delay) << ","
            << std::string(sparkStatusNames.at(entry.coils12.spark_status)) << ","
            << std::to_string(entry.coils12.peak_p_in) << ","
            << std::to_string(entry.coils12.peak_n_in) << ","
            << std::to_string(entry.coils12.peak_p_out) << ","
            << std::to_string(entry.coils12.peak_n_out) << ","
            << std::string(softStartStatusNames.at(entry.coils12.sstart_status)) << ","
            << std::to_string(entry.coils34.n_events) << ","
            << std::to_string(entry.coils34.spark_delay) << ","
            << std::string(sparkStatusNames.at(entry.coils34.spark_status)) << ","
            << std::to_string(entry.coils34.peak_p_in) << ","
            << std::to_string(entry.coils34.peak_n_in) << ","
            << std::to_string(entry.coils34.peak_p_out) << ","
            << std::to_string(entry.coils34.peak_n_out) << ","
            << std::string(softStartStatusNames.at(entry.coils34.sstart_status)) << ","
            << std::to_string(entry.eng_rpm) << ","
            << std::to_string(entry.adc_read_time) << ","
            << std::to_string(entry.n_queue_errors);
        ofs << std::endl;
        ofs.flush();
    }

    ofs.close();
    LOG_INFO("Ignition Box history saved to Flash, records written: ", history.size());
}