#include "can.hpp"

#include <cstdio>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>

#include <pb_decode.h>

#include "configuration.pb.h"
#include "ctrl.pb.h"
#include "temperature.pb.h"

const struct device *can_dev = DEVICE_DT_GET(DT_NODELABEL(fdcan1));

namespace rims {

// External
CAN_MSGQ_DEFINE(can_engress_queue, 5);
CAN_MSGQ_DEFINE(can_ingress_queue, 5);

// IPC
K_MSGQ_DEFINE(can_temperature_ingress_queue, temperature_IngressMessage_size, 2, 1);
K_MSGQ_DEFINE(can_config_ingress_queue, config_IngressMessage_size, 2, 1);
K_MSGQ_DEFINE(can_ctrl_ingress_queue, ctrl_IngressMessage_size, 2, 1);

void CAN_RX::loop() {
    struct can_frame frame;
    int              filter_id;

    /* Set up a filter to receive all CAN messages (example: ID mask 0x000, all pass) */
    struct can_filter filter = {
        .id    = 0,
        .mask  = CAN_EXT_ID_MASK, /* No filter */
        .flags = CAN_FILTER_IDE,
    };

    filter_id = can_add_rx_filter_msgq(can_dev, &can_ingress_queue, &filter);
    if (filter_id < 0) {
        printk("Failed to add CAN RX filter (err %d)", filter_id);
        return;
    }
    printk("CAN RX filter added (id: %d)", filter_id);

    while (1) {
        k_msgq_get(&can_ingress_queue, &frame, K_FOREVER);

        if (IS_ENABLED(CONFIG_CAN_ACCEPT_RTR) && (frame.flags & CAN_FRAME_RTR) != 0U) {
            continue;
        }

        if (frame.dlc != 2U) {
            printf("Wrong data length: %u\n", frame.dlc);
            continue;
        }

        printf("CAN received   :~%u bytes\n", can_dlc_to_bytes(frame.dlc));
        printf("CAN received id: %u\n", frame.id);

        bool ok     = false;
        auto stream = pb_istream_from_buffer(frame.data, can_dlc_to_bytes(frame.dlc));

        const auto decodeAndPublish = [&](const pb_msgdesc_t *fields, void *dest, k_msgq &msgq) {
            ok = pb_decode(&stream, fields, &dest);
            if (ok) {
                printf("Decode OK");
                while (k_msgq_put(&msgq, &dest, K_NO_WAIT)) {
                    k_msgq_purge(&msgq);
                }
            } else {
                /// TODO loging
                printf("Decode failed");
            }
        };

        switch (frame.id) {
        case 0x01: { /// TODO temperature endpoint ID
            temperature_IngressMessage msg;
            decodeAndPublish(temperature_IngressMessage_fields, &msg, can_temperature_ingress_queue);
            break;
        }
        case 0x02: /// TODO configuration endpoint ID
        {
            config_IngressMessage msg;
            decodeAndPublish(config_IngressMessage_fields, &msg, can_config_ingress_queue);
            break;
        }
        case 0x03: /// TODO phase controll endpoint ID
        {
            ctrl_IngressMessage msg;
            decodeAndPublish(ctrl_IngressMessage_fields, &msg, can_ctrl_ingress_queue);
            break;
        }
        default:
            break;
        }

        if (not ok) {
            printk("data broken");
        }
    }

    /* Remove the filter if the thread exits (unlikely here) */
    can_remove_rx_filter(can_dev, filter_id);
}

void CAN_TX::loop() {
    while (1) {
        can_frame frame;
        // Wait for a message in the FIFO
        auto status = k_msgq_get(&can_engress_queue, &frame, K_SECONDS(1));

        if (status == 0) {
            int ret = can_send(can_dev, &frame, K_FOREVER, NULL, NULL);
            if (ret == 0) {
                printk("CAN message sent successfully.\n");
            } else {
                printk("Failed to send CAN message, error: %d\n", ret);
            }
        } else if (status == -EAGAIN) {
            printk("Nothing to send, looping");
        }
    }
}

bool CAN_RX::Setup() {
    // CAN device pointer

    if (!can_dev) {
        printk("Failed to get CAN device binding.\n");
        return false;
    }

    /// TODO CAN set timing https://docs.zephyrproject.org/latest/hardware/peripherals/can/controller.html

    // Start CAN device
    int ret = can_start(can_dev);
    if (ret != 0) {
        printk("Failed to start CAN device, error: %d\n", ret);
        return false;
    }
    return true;
}

void CAN_TX::send(uint32_t senderId, const void *data, std::size_t messageLength) {
    /// TODO assert proper ID
    /// TODO assert message_length
    can_frame frame;

    // Populate CAN frame with sample data
    memcpy(frame.data, data, messageLength);

    frame.flags = CAN_FRAME_FDF | CAN_FRAME_BRS;   // use CAN FD
    frame.id    = senderId;                        // ID of given service
    frame.dlc   = can_bytes_to_dlc(messageLength); // CAN data length code

    // put data on outgoing messagesqueue

    /// TODO fix CANBUS
    // k_msgq_put(&can_engress_queue, &frame, K_MSEC(50));
    /// TODO handle error
}

} // namespace rims