#include "canbus.h"
#include <zephyr/logging/log.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/byteorder.h>
#include "config.h"
#include "modbus.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, CANBUS_RX_MSGQ_SIZE);

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 = 0,                           // No special flags,
        .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;
    extern struct k_msgq modbus_msgq;

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

        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

        if (IS_ENABLED(CONFIG_CAN_ACCEPT_RTR) && (frame.flags & CAN_FRAME_RTR) != 0U)
        {
            LOG_DBG("Received RTR frame for register address: 0x%02x", target_register);
            switch (target_register)
            {
            case CANBUS_REGISTER_WATER_LEVEL_MM:
            case CANBUS_REGISTER_WATER_MINIMUM:
            case CANBUS_REGISTER_WATER_MAXIMUM:
                LOG_DBG("Handling water sensor request for node %d", target_node);
                modbus_register_t reg_max;
                reg_max.reg = target_register;
                reg_max.command = CANBUS_REGISTER_COMMAND_GET; // Set command to GET
                k_msgq_put(&modbus_msgq, &reg_max, K_NO_WAIT);
                break;
            default:
                LOG_WRN("Received RTR frame for unknown register address: 0x%02x", target_register);
            }
            continue;
        }
        // Log the received frame details
        LOG_DBG("Received CAN frame: ID=0x%08x, DLC=%u, Flags=0x%02x, Target Node=%d, Register Address=0x%02x",
                frame.id, frame.dlc, frame.flags, target_node, target_register);
        switch (target_register)
        {
        case CANBUS_REGISTER_WATER_MINIMUM:
        case CANBUS_REGISTER_WATER_MAXIMUM:
            LOG_DBG("Handling water sensor data for node %d", target_node);
            modbus_register_t reg;
            reg.reg = target_register;
            reg.command = CANBUS_REGISTER_COMMAND_SET; // Set command to SET
            if (frame.dlc >= sizeof(reg.value)) // Ensure enough data length for value
            {
                reg.value = sys_get_be16(frame.data); // Convert received data to big-endian format
                LOG_DBG("Received value: %d, 0x%04x", reg.value, reg.value);
            }
            else
            {
                LOG_ERR("Received frame with insufficient data length: %u", frame.dlc);
                continue; // Skip processing if data length is invalid
            }
            k_msgq_put(&modbus_msgq, &reg, K_NO_WAIT);
            break;
        }
        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_register(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;
}

int canbus_request_register(uint8_t node_id, uint8_t register_address)
{
    int rc;
    struct can_frame frame;

    // Prepare the CAN frame for the request
    frame.id = (node_id << 8) | register_address; // Standard ID with node ID in the first byte
    frame.dlc = 0;                                // No data for request
    frame.flags = CAN_FRAME_RTR;                  // Set RTR flag for request

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

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