#include "mcp2521_hardware_handle.hpp"

#ifdef ESP_PLATFORM
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "mcp2521_hardware_esp.hpp"

static void IRAM_ATTR gpio_isr_can_handler(void* arg) {
    MCP2521_HardwareHandle_ESP * handle = (MCP2521_HardwareHandle_ESP *)arg;
    handle->isr_can_interrupt();
}

static void handleInteruptTaskCallerFn(void *arg) {
    MCP2521_HardwareHandle_ESP * handle = (MCP2521_HardwareHandle_ESP *)arg;
    handle->handleIntteruptTaskFn();
}

void MCP2521_HardwareHandle_ESP::initPins(
    gpio_num_t int_pin
) {
    canInterruptSemaphore = xSemaphoreCreateBinary();

    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_INTR_NEGEDGE;
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pin_bit_mask = 1 << int_pin;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    gpio_config(&io_conf);

    gpio_install_isr_service(0);
    gpio_isr_handler_add(int_pin, gpio_isr_can_handler, this);

    xTaskCreatePinnedToCore(
        (TaskFunction_t)&handleInteruptTaskCallerFn,
        "canInterruptTask",
        2048,
        this,
        5,
        &canInterruptTaskHandle,
        0
    );
}

void MCP2521_HardwareHandle_ESP::handleIntteruptTaskFn() {
    while(true) {
        xSemaphoreTake(canInterruptSemaphore, portMAX_DELAY);
        intHandler(intHandlerArg);
    }
}

void MCP2521_HardwareHandle_ESP::isr_can_interrupt() {
    BaseType_t wokenTask = pdFALSE;
    xSemaphoreGiveFromISR(canInterruptSemaphore, &wokenTask);

    if(wokenTask) {
        portYIELD_FROM_ISR();
    }
}

void MCP2521_HardwareHandle_ESP::registerIntHandler(intHandlerFunction_t handler, void * arg) {
    intHandlerArg = arg;
    intHandler = handler;
}

#endif