/*
 * canbus_service.c
 *
 *  Created on: 28 Apr 2026
 *      Author: Christian Lind Vie Madsen
 */
#include "canbus_service.h"
#include <stdio.h>

#include "../board_defines/board_defines.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "../mqtt_service/mqtt_driver.h"

#include "esp_log.h"

#define TAG "canbus_service.c"

#define RX_TASK_STACK     4096
#define RX_TASK_PRIORITY  9

#define TX_QUEUE_LEN      32
#define RX_QUEUE_LEN      32

#define MAX_RETRIES       3
#define TX_TIMEOUT_MS     10

// ---------------- STATE ----------------
static QueueHandle_t tx_queue = NULL;
//static can_rx_callback_t rx_callback = NULL;

// ---------------- DRIVER INIT ----------------
static esp_err_t can_driver_init(void)
{
    twai_general_config_t g_config =
        TWAI_GENERAL_CONFIG_DEFAULT(GPIO_CANBUS_TX, GPIO_CANBUS_RX, TWAI_MODE_NORMAL);

    twai_timing_config_t t_config = TWAI_TIMING_CONFIG_50KBITS();
    twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

    g_config.rx_queue_len = RX_QUEUE_LEN;
    g_config.tx_queue_len = TX_QUEUE_LEN;

    ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config, &f_config));
    ESP_ERROR_CHECK(twai_start());

    ESP_LOGI(TAG, "TWAI initialized");
    return ESP_OK;
}

// ---------------- TX FUNCTION ----------------
static esp_err_t can_transmit_with_retry(const twai_message_t *msg)
{
    int retries = 0;

    while (retries < MAX_RETRIES) {
        esp_err_t err = twai_transmit(msg, pdMS_TO_TICKS(TX_TIMEOUT_MS));

        if (err == ESP_OK) {
            return ESP_OK;
        }

        if (err == ESP_ERR_TIMEOUT) {
            retries++;
            vTaskDelay(pdMS_TO_TICKS(5));
        } else {
            ESP_LOGE(TAG, "TX failed: %s", esp_err_to_name(err));
            return err;
        }
    }

    ESP_LOGW(TAG, "TX dropped after retries");
    return ESP_FAIL;
}

void publish_can_message(const twai_message_t *msg)
{
    if (!msg) return;

    char payload[160];
    int pos = 0;

    // header
    pos += snprintf(payload + pos, sizeof(payload) - pos,
        "{\"ts\":%lu,\"id\":%u,\"extd\":%d,\"rtr\":%d,\"dlc\":%d,\"data\":[",
        (unsigned long)esp_log_timestamp(),
        msg->identifier,
        msg->extd,
        msg->rtr,
        msg->data_length_code
    );

    // data bytes (ONLY valid DLC bytes)
    for (int i = 0; i < msg->data_length_code; i++) {
        pos += snprintf(payload + pos, sizeof(payload) - pos,
            "%u%s",
            msg->data[i],
            (i < msg->data_length_code - 1) ? "," : ""
        );
    }

    // close JSON
    snprintf(payload + pos, sizeof(payload) - pos, "]}");

    mqtt_driver_publish("can/raw", payload);

    ESP_LOGD(TAG, "CAN ID 0x%X DLC %d",
             msg->identifier,
             msg->data_length_code);
}

// ---------------- RX TASK ----------------
static void can_rx_task(void *arg)
{
    twai_message_t msg;

    while (1) {
        if (twai_receive(&msg, portMAX_DELAY) == ESP_OK) {

            // Pass to application
            publish_can_message(&msg);
        	//futedo_can_decoder(&futedo_conf, &msg.data[0], msg.data_length_code, msg.identifier);

        	// Print data:
        	ESP_LOGI(TAG, "ID: 0x%x; [0]: 0x%x, [1]: 0x%x, [2]: 0x%x,[3]: 0x%x, [4]: 0x%x, [5]: 0x%x,",msg.identifier,msg.data[0],msg.data[1],msg.data[2],msg.data[3],msg.data[4],msg.data[5]);
        }
    }
}

// ---------------- TX TASK ----------------
static void can_tx_task(void *arg)
{
    twai_message_t msg;

    while (1) {
        if (xQueueReceive(tx_queue, &msg, portMAX_DELAY) == pdTRUE) {
            can_transmit_with_retry(&msg);
        }
    }
}

// ---------------- PUBLIC API ----------------

// Init service
void canbus_service_init()
{

    can_driver_init();

    tx_queue = xQueueCreate(TX_QUEUE_LEN, sizeof(twai_message_t));

    if (!tx_queue) {
        ESP_LOGE(TAG, "Failed to create TX queue");
        return;
    }

    xTaskCreate(can_rx_task, "can_rx_task", RX_TASK_STACK, NULL, RX_TASK_PRIORITY, NULL);
    xTaskCreate(can_tx_task, "can_tx_task", 2048, NULL, RX_TASK_PRIORITY - 1, NULL);
}

// Send message (non-blocking)
esp_err_t canbus_send(const twai_message_t *msg)
{
    if (xQueueSend(tx_queue, msg, 0) != pdTRUE) {
        ESP_LOGW(TAG, "TX queue full");
        return ESP_FAIL;
    }
    return ESP_OK;
}