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

//// GLOBAL STATIC FUNCTIONS

// Timeout callback for microsecond precision
void 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;
    while (cls->m_running)
    {
        cls->run();
    }
}

// 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.get();
    ADS1256 *adc = params->name == "rtIgnTask_A" ? dev->m_adc_a.get() : dev->m_adc_b.get();
    std::mutex &spi_mutex = params->name == "rtIgnTask_A" ? dev->m_spi_a_mutex : dev->m_spi_b_mutex;
    ExternalIO *io = dev->m_ext_io.get();

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

    LOG_DEBUG("rtTask HDL Params OK, HDL* [", (uint32_t)rt_task_info.xHandle, "]");
    LOG_DEBUG("rtTask ISR Params OK, ISR* [", (uint32_t)rt_int.isr_ptr, "]");
    LOG_DEBUG("rtTask QUE Params OK, QUE* [", (uint32_t)rt_queue, "]");

    // 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"};
    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 [", 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
        {
            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;
            }
        }
    }
    // Delete the 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_fs_mutex(fs_mutex), m_core(core), m_max_history(history_size)
{
    // create queue buffers
    m_queue = xQueueCreate(queue_size, sizeof(ignitionBoxStatus));
    if (!m_queue)
    {
        LOG_ERROR("Unable To Create Task [", params.name.c_str(), "] queues");
        m_manager_status = rtTaskStatus::ERROR;
        return;
    }
    else
        m_params.rt_queue = m_queue;

    // create PSram history vectors
    m_history_0 = PSHistory(history_size);
    m_history_1 = PSHistory(history_size);
    // assing active and writable history
    m_active_history = std::unique_ptr<PSHistory>(&m_history_0);
    m_save_history = std::unique_ptr<PSHistory>(&m_history_1);

    LOG_WARN("Starting Manager for [", m_params.name.c_str(), "]");
    // auto task_success = pdPASS;
    auto task_success = xTaskCreatePinnedToCore(
        rtIgnitionTask_manager,
        (std::string("man_") + m_params.name).c_str(),
        8192,
        (void *)this,
        m_params.rt_priority >> 2,
        &m_manager_handle,
        m_core);

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

    // average every 10 samples
    m_info_filtered = ignitionBoxStatusFiltered(10);
    m_last_data = millis();
    m_manager_status = rtTaskStatus::OK;
}

rtIgnitionTask::~rtIgnitionTask()
{
    if (m_rt_handle)
        vTaskDelete(m_rt_handle);
    if (m_manager_handle)
        vTaskDelete(m_manager_handle);
    if (m_queue)
        vQueueDelete(m_queue);
}

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

    if (new_data == pdPASS)
    {
        m_last_data = millis();
        m_manager_status = rtTaskStatus::RUNNING;
        // if history buffer is full swap buffers and if enabled save history buffer
        if (m_counter_status >= m_active_history->size())
        {
            LOG_DEBUG("Save for Buffer Full: ", m_counter_status);
            m_counter_status = 0;
            m_partial_save = false; // reset partial save flag on new data cycle
            std::swap(m_active_history, m_save_history);
            if (m_enable_save)
                saveHistory(*m_save_history, m_history_path); // directly call the save task function to save without delay
        }

        // update filtered data
        m_info_filtered.update(m_last_status);
        (*m_active_history)[m_counter_status] = m_last_status;

        if (m_on_message_cb && m_counter_status % 10 == 0)
        {
            m_on_message_cb(m_info_filtered);
        }

        // update data counter
        m_counter_status++;
    }
    else
    {
        if (millis() - m_last_data > c_idle_time)
        {
            if (m_counter_status > 0 && !m_partial_save)
            {
                LOG_DEBUG("Save Partial: ", m_counter_status);
                m_active_history->resize(m_counter_status);
                saveHistory(*m_active_history, m_history_path);
                m_active_history->resize(m_max_history);
                m_counter_status = 0;
                m_partial_save = true;
            }
            m_manager_status = rtTaskStatus::IDLE;
        }
        delay(5); // yeld to another task
    }
}

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_rt_handle,
        m_core);
    const bool success = task_success == pdPASS && m_rt_handle != nullptr;
    if (success)
        m_manager_status = rtTaskStatus::IDLE;
    return success;
}

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

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

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

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

void rtIgnitionTask::enableSave(const bool enable, const std::filesystem::path filename)
{
    m_enable_save = enable;
    if (enable && !filename.empty())
    {
        LOG_WARN("Save History Enabled Task [", m_params.name.c_str(), "]");
        m_history_path = 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_on_message_cb = 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_fs_mutex);

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