//
// can
//

#include "can.h"
#include "esp_log.h"
#include "driver/twai.h"

// EMZ FIXME
#define TX_GPIO_NUM 5
#define RX_GPIO_NUM 6
#define RX_TASK_PRIO 3


// Private variables
static const char *TAG = "canbus";


// static QueueHandle_t tx_task_queue;
// static QueueHandle_t rx_task_queue;


// Task for receiving CAN messages
static void twai_receive_task(void *arg)
{
    while (1) {
        twai_message_t rx_msg;
        twai_receive(&rx_msg, portMAX_DELAY);
        if (rx_msg.identifier == 0xdead) {
            ESP_LOGI(TAG, "Received data ");
        }

    }
    vTaskDelete(NULL);
}


// Initialize the CAN bus
void can_init(void)
{
    // CAN bus configuration
    twai_timing_config_t t_config = TWAI_TIMING_CONFIG_125KBITS();
    twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
    twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TX_GPIO_NUM, RX_GPIO_NUM, TWAI_MODE_NORMAL);
	
    // Move canbus irq to free up the default level 1 IRQ it will take up (processor panics if we don't do this)
    g_config.intr_flags = ESP_INTR_FLAG_LOWMED;

    // Install TWAI driver
    ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config, &f_config));
    ESP_LOGI(TAG, "Driver installed");
    ESP_ERROR_CHECK(twai_start());
    ESP_LOGI(TAG, "Driver started");

    // rx_task_queue = xQueueCreate(1, sizeof(rx_task_action_t));
    // tx_task_queue = xQueueCreate(1, sizeof(tx_task_action_t));
    xTaskCreatePinnedToCore(twai_receive_task, "TWAI_rx", 4096, NULL, RX_TASK_PRIO, NULL, tskNO_AFFINITY);
}


// Send message on the CAN bus
void can_send(uint32_t id, uint8_t* data, uint8_t len, uint32_t timeout)
{
    // Gross
    twai_message_t msg = { .identifier = id, .data_length_code = len, .ss = 1, .data = {data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7]} };
    twai_transmit(&msg, timeout);
}