#include "canbus.h"
#include <zephyr/logging/log.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/byteorder.h>
#include "config.h"

int canbus_node_id = 1; // Default node ID for CAN bus

const struct device *const can_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_canbus));

struct k_thread rx_thread_data;

K_THREAD_STACK_DEFINE(rx_thread_stack, CANBUS_RX_THREAD_STACK_SIZE);
CAN_MSGQ_DEFINE(commands_msq, 2);

LOG_MODULE_REGISTER(canbus, CONFIG_LOG_CAN_LEVEL);

static void rx_thread(void *arg1, void *arg2, void *arg3)
{
    ARG_UNUSED(arg1);
    ARG_UNUSED(arg2);
    ARG_UNUSED(arg3);
    const struct can_filter filter = {
        .flags = CAN_FILTER_IDE,
        .id = (canbus_node_id << 8) | 0x00,   // Standard ID with node ID in the first byte
        .mask = CAN_STD_ID_MASK & 0xFFFFFF00U // Mask for standard ID, ignoring the last byte
    };

    struct can_frame frame;
    int filter_id;

    filter_id = can_add_rx_filter_msgq(can_dev, &commands_msq, &filter);
    LOG_DBG("RX thread started. Filter ID: %d, flags: 0x%02x, id: 0x%08x, mask: 0x%08x", filter_id, filter.flags, filter.id, filter.mask);

    while (1)
    {
        k_msgq_get(&commands_msq, &frame, K_FOREVER);
        LOG_DBG("Received CAN frame: ID=0x%08x, DLC=%u, Flags=0x%02x",
                frame.id, frame.dlc, frame.flags);
        if (IS_ENABLED(CONFIG_CAN_ACCEPT_RTR) && (frame.flags & CAN_FRAME_RTR) != 0U)
        {
            continue;
        }

        uint8_t target_node = (frame.id >> 8) & 0xFF; // Extract the target node from the ID
        if (target_node != canbus_node_id)
        {
            LOG_WRN("Received frame for different node ID: %d, expected: %d", target_node, canbus_node_id);
            continue; // Ignore frames not meant for this node
        }
        uint8_t target_register = frame.id & 0xFF; // Extract the register address from the ID

        // Log the received frame details
        LOG_DBG("Received CAN frame: ID=0x%08x, DLC=%u, Flags=0x%02x, Target Node=%d, Register Address=%d",
                frame.id, frame.dlc, frame.flags, target_node, target_register);
        LOG_HEXDUMP_DBG(frame.data, sizeof(frame.data) * sizeof(frame.data[0]), "Frame data:");
    }
}

static void canbus_tx_callback(const struct device *dev, int error, void *user_data)
{
    ARG_UNUSED(dev);
    ARG_UNUSED(user_data);

    if (error != 0)
    {
        LOG_ERR("CAN transmission error: %d", error);
    }
    else
    {
        LOG_DBG("CAN message sent successfully");
    }
}
int canbus_init(void)
{
    k_tid_t rx_tid;
    int rc;

    rc = settings_get_val_len("canbus/node_id");
    if (rc < 0)
    {
        LOG_ERR("Failed to check CAN bus settings: %d", rc);
        return rc;
    }

    else if (rc == 0)
    {
        LOG_WRN("No CAN bus node id found in settings, using default (%d)", canbus_node_id);
        settings_save_one("canbus/node_id", &canbus_node_id, sizeof(canbus_node_id));
        if (rc < 0)
        {
            LOG_ERR("Failed to save default CAN bus node id: %d", rc);
            return rc;
        }
    }
    else
    {
        LOG_DBG("CAN bus node ID found in settings: %d", canbus_node_id);
    }

    if (!device_is_ready(can_dev))
    {
        LOG_ERR("CAN device %s is not ready", can_dev->name);
        return -ENODEV;
    }

#ifdef CONFIG_LOOPBACK_MODE
    rc = can_set_mode(can_dev, CAN_MODE_LOOPBACK);
    if (rc != 0)
    {
        LOG_ERR("Failed to set CAN loopback mode: %d", rc);
        return rc;
    }
#endif
	rc = can_start(can_dev);
	if (rc != 0) {
		printf("Error starting CAN controller [%d]", rc);
		return 0;
	}
    LOG_DBG("CAN device %s is ready", can_dev->name);

    LOG_DBG("Trying to start rx thread");

    // Create the RX thread for handling incoming CAN messages
    rx_tid = k_thread_create(&rx_thread_data, rx_thread_stack,
                             K_THREAD_STACK_SIZEOF(rx_thread_stack),
                             rx_thread, NULL, NULL, NULL,
                             CANBUS_RX_THREAD_PRIORITY, 0, K_NO_WAIT);

    if (rx_tid == NULL)
    {
        LOG_ERR("Failed to create CAN RX thread");
        return -ENOMEM; // Not enough memory to create thread
    }

    LOG_INF("CAN bus initialized with node ID: %d,", canbus_node_id);
    return 0; // Return 0 on success
}

int canbus_send_message(uint8_t destination_node, uint8_t register_address, uint8_t *data, size_t data_length)
{
    struct can_frame frame;
    if (data_length > sizeof(frame.data))
    {
        LOG_ERR("Data length exceeds maximum CAN frame size");
        return -EINVAL; // Invalid argument
    }

    frame.id = (destination_node << 8) | register_address; // Standard ID with node ID in the first byte
    frame.dlc = data_length;
    frame.flags = 0; // No special flags

    // Copy data into the frame
    memcpy(frame.data, data, data_length);

    // Send the CAN frame
    int rc = can_send(can_dev, &frame, K_MSEC(100), canbus_tx_callback, NULL);
        if (rc < 0) {
        LOG_ERR("Failed to send CAN message: %d", rc);
        return rc; // Return error code
    }

    LOG_DBG("CAN message sent: ID=0x%08x, DLC=%u", frame.id, frame.dlc);
    return 0;
}