#include "canbus.h"
#include <zephyr/logging/log.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/drivers/can.h>
#include <stdlib.h>

#ifdef CONFIG_HAS_MODBUS_WATERLEVEL_SENSOR
#include "waterlevel_sensor.h"
#endif // CONFIG_HAS_MODBUS_WATERLEVEL_SENSOR

#ifdef CONFIG_HAS_VALVE
#include "valve.h"
#endif // CONFIG_HAS_VALVE

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

LOG_MODULE_REGISTER(canbus, CONFIG_LOG_CAN_LEVEL);

K_MSGQ_DEFINE(canbus_msgq, sizeof(struct can_frame), CANBUS_RX_MSGQ_SIZE, 4);
K_THREAD_STACK_DEFINE(canbus_rx_stack, CANBUS_RX_THREAD_STACK_SIZE);

uint8_t node_id = 0; // Default node ID, can be set later
uint8_t can_msg_id = 0;

k_tid_t canbus_rx_thread_id;
struct k_thread canbus_rx_thread_data;

int canbus_rx_filter_id = -1;

void canbus_rx_callback(const struct device *dev, struct can_frame *frame, void *user_data)
{
    int rc;
    ARG_UNUSED(dev);
    ARG_UNUSED(user_data);

    if (frame->id >> 8 != node_id) // Check if the frame ID matches the node ID
    {
        LOG_DBG("Received CAN frame with ID %d, but it does not match node ID %d", frame->id >> 8, node_id);
        return; // Ignore frames that do not match the node ID
    }

    // Process the received CAN frame
    rc = k_msgq_put(&canbus_msgq, frame, K_NO_WAIT);
    if (rc != 0)
    {
        LOG_ERR("Failed to put CAN frame into message queue: %d", rc);
    }
}

void canbus_thread(void *arg1, void *arg2, void *arg3)
{
    ARG_UNUSED(arg1);
    ARG_UNUSED(arg2);
    ARG_UNUSED(arg3);

    LOG_INF("CAN bus thread started");

    // Main loop for CAN bus operations
    while (1)
    {
        int rc;
        struct can_frame frame;
        k_msgq_get(&canbus_msgq, &frame, K_FOREVER); // Wait for a message from the queue
        LOG_DBG("Received CAN frame with ID: 0x%02x, DLC: %d, RTR: %d",
                frame.id, frame.dlc, (uint8_t)(frame.flags & CAN_FRAME_RTR));
        LOG_HEXDUMP_DBG(frame.data, frame.dlc, "CAN frame data");
        uint8_t reg = frame.id & 0xFF;               // Extract register from the frame ID
        bool is_rtr = (frame.flags & CAN_FRAME_RTR) != 0; // Check if it's a remote transmission request
        switch (reg)
        {
#ifdef CONFIG_HAS_MODBUS_WATERLEVEL_SENSOR
        case CANBUS_REG_WATERLEVEL_LEVEL:
        case CANBUS_REG_WATERLEVEL_ZERO_POINT:
        case CANBUS_REG_WATERLEVEL_MAX_POINT:
            waterlevel_command_t command;
            command.cmd = is_rtr ? WATERLEVEL_CMD_GET : WATERLEVEL_CMD_SET; // Determine command type based on RTR
            command.reg = reg;                                              // Set the register ID
            int16_t value = 0; // Initialize value to 0
            if (!is_rtr) // If it's not a remote request, extract the value from the frame data
            {
                if (frame.dlc < sizeof(command.data))
                {                    LOG_ERR("Received frame with insufficient data length: %d", frame.dlc);
                    continue; // Skip processing if data length is insufficient
                }
                value = sys_be16_to_cpu(*(uint16_t *)frame.data); // Convert data from big-endian to host byte order
                command.data = value; // Set the data field
                LOG_DBG("Setting command data to: %d", value);
            }
            extern struct k_msgq waterlevel_sensor_msgq;                    // Declare the water level sensor message queue
            k_msgq_put(&waterlevel_sensor_msgq, &command, K_NO_WAIT);
            break;
#endif // CONFIG_HAS_MODBUS_WATERLEVEL_SENSOR
#ifdef CONFIG_HAS_VALVE
        case CANBUS_REG_VALVE_STATUS:
        case CANBUS_REG_VALVE_OPERATION:
        if (is_rtr)
            {
                LOG_DBG("Received remote request for valve status or operation");
                if (reg == CANBUS_REG_VALVE_STATUS)
                {
                    valve_send_status(); // Send the current valve status
                }
                else if (reg == CANBUS_REG_VALVE_OPERATION)
                {
                    valve_send_operation(); // Send the current valve operation state
                } else {
                    LOG_ERR("Unknown valve register: 0x%02x", reg);
                    continue; // Skip processing if the register is unknown
                }
            }
            else
            {
                LOG_ERR("Received CAN frame with data for valve status or operation, but RTR is not set");
                continue; // Skip processing if RTR is not set for valve status or operation
            }
            break;
        case CANBUS_REG_VALVE_COMMAND:
        {
            if (is_rtr) {
                LOG_ERR("Received remote request for valve command, but this is not supported");
                continue; // Skip processing if RTR is set for valve command
            } else {
                // Extract the command from the frame data
                if (frame.dlc < sizeof(uint8_t)) {
                    LOG_ERR("Received frame with insufficient data length for valve command: %d", frame.dlc);
                    continue; // Skip processing if data length is insufficient
                }
                uint8_t command = frame.data[0]; // Get the command from the first byte of data
                LOG_DBG("Received valve command: 0x%02x", command);
                rc = valve_cmd(command); // Send the command to the valve
                if (rc < 0) {
                    LOG_ERR("Failed to send valve command: %d", rc);
                    continue; // Skip processing if sending the command failed
                }
            }
            break;
        }
        default:
            LOG_DBG("Received CAN frame with unknown register: 0x%02x", reg);
            break;
        }
        #endif // CONFIG_HAS_VALVE
    }
}

int canbus_init(void)
{
    int rc = 0;

    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);
    // Initialize the CAN RX thread
    canbus_rx_thread_id = k_thread_create(&canbus_rx_thread_data, canbus_rx_stack,
                                          K_THREAD_STACK_SIZEOF(canbus_rx_stack), canbus_thread,
                                          NULL, NULL, NULL,
                                          CANBUS_RX_THREAD_PRIORITY, 0, K_NO_WAIT);
    k_thread_name_set(canbus_rx_thread_id, "canbus_rx");

    const struct can_filter filter = {
        .id = node_id << 8, // Standard ID with node ID in the first byte
        .mask = 0x700,      // Mask to match the first byte of the ID
        .flags = 0,         // No special flags
    };
    canbus_rx_filter_id = can_add_rx_filter(can_dev, canbus_rx_callback, NULL, &filter);
    LOG_DBG("CAN RX filter added for node ID %d", canbus_rx_filter_id);
    return 0;
}

void canbus_tx8_callback(const struct device *dev, int error, void *user_data)
{
    ARG_UNUSED(dev);
    uint8_t frame_id = *(uint8_t *)user_data; // Retrieve the frame ID from user data

    if (error != 0)
    {
        LOG_ERR("CAN transmission error. Error code: %d, Frame ID: %d", error, frame_id);
    }
    else
    {
        LOG_DBG("CAN message with Frame ID %d sent successfully", frame_id);
    }
    free(user_data); // Free the allocated memory for frame ID
}

int canbus_send8(uint16_t reg, uint8_t value)
{
    int rc = 0;
    struct can_frame frame = {
        .id = (node_id << 8) | reg, // Standard ID with node ID in the first byte
        .dlc = sizeof(value),        // Data Length Code (DLC)
    };
    frame.data[0] = value; // Set the value in the first byte of the data
    uint8_t *frame_id = malloc(sizeof(uint8_t)); // Allocate memory for frame ID
    LOG_DBG("Preparing to send 8-bit value 0x%02x to register 0x%02x on node %d", value, reg, node_id);
    if (frame_id == NULL)
    {
        LOG_ERR("Failed to allocate memory for frame ID");
        return -ENOMEM; // Not enough memory
    }
    *frame_id = can_msg_id++; // Increment message ID for uniqueness
    LOG_DBG("Using frame ID: %d", *frame_id);
    rc = can_send(can_dev, &frame, CANBUS_TX_TIMEOUT, canbus_tx8_callback, frame_id);
    // Send the CAN frame with a timeout and callback
    if (rc != 0)
    {
        LOG_ERR("Failed to queue CAN frame: %d", rc);
        free(frame_id); // Free the allocated memory for frame ID
        return rc;
    }
    return 0;
}

void canbus_tx16_callback(const struct device *dev, int error, void *user_data)
{
    ARG_UNUSED(dev);
    uint8_t frame_id = *(uint8_t *)user_data; // Retrieve the frame ID from user data

    if (error != 0)
    {
        LOG_ERR("CAN transmission error. Error code: %d, Frame ID: %d", error, frame_id);
    }
    else
    {
        LOG_DBG("CAN message with Frame ID %d sent successfully", frame_id);
    }
    free(user_data); // Free the allocated memory for frame ID
}

int canbus_send16(uint16_t reg, uint16_t value)
{
    int rc = 0;
    union data_type
    {
        int16_t value;
        uint8_t bytes[2];
    } data;
    data.value = sys_cpu_to_be16(value); // Convert value to big-endian format

    struct can_frame frame = {
        .id = (node_id << 8) | reg, // Standard ID with node ID in the first byte
        .dlc = sizeof(data),        // Data Length Code (DLC)
    };
    memcpy(frame.data, data.bytes, sizeof(data)); // Copy data into the frame
    uint8_t *frame_id = malloc(sizeof(uint8_t));  // Allocate memory for frame ID
    LOG_DBG("Preparing to send 16-bit value 0x%04x to register 0x%02x on node %d", value, reg, node_id);
    if (frame_id == NULL)
    {
        LOG_ERR("Failed to allocate memory for frame ID");
        return -ENOMEM; // Not enough memory
    }
    *frame_id = can_msg_id++; // Increment message ID for uniqueness
    LOG_DBG("Using frame ID: %d", *frame_id);
    rc = can_send(can_dev, &frame, CANBUS_TX_TIMEOUT, canbus_tx16_callback, frame_id);
    // Send the CAN frame with a timeout and callback
    if (rc != 0)
    {
        LOG_ERR("Failed to queue CAN frame: %d", rc);
        free(frame_id); // Free the allocated memory for frame ID
        return rc;
    }

    LOG_DBG("Queued 16-bit value 0x%04x to register 0x%02x on node %d, frame ID: %d",
            value, reg, node_id, *frame_id);
    return 0;
}