#include "microros_node.hpp"

#include <zephyr/kernel.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <zephyr/sys/slist.h>
#include <rosidl_runtime_c/message_type_support_struct.h>
#include <stdlib.h>  // for malloc/free

MicrorosExecutor::MicrorosExecutor(const char *name) {
    this->node_name = name;
    this->allocator = rcl_get_default_allocator();

    // Initialize empty lists
    sys_slist_init(&this->publishers_head);
    sys_slist_init(&this->subscribers_head);
}

MicrorosExecutor::~MicrorosExecutor() {
    // Free all publishers
    struct publisher_node *pub;
    SYS_SLIST_FOR_EACH_CONTAINER(&this->publishers_head, pub, node) {
        rcl_publisher_fini(&pub->publisher, &this->node);
        free(pub);
    }

    // Free all subscribers
    struct subscription_node *sub;
    SYS_SLIST_FOR_EACH_CONTAINER(&this->subscribers_head, sub, node) {
        rcl_subscription_fini(&sub->subscription, &this->node);
        free(sub);
    }

    rcl_node_fini(&this->node);
}

bool MicrorosExecutor::connect() {
    static bool support_init = false;
    static bool defaults_init = false;
    static bool executor_init = false;

    if (!support_init){
        if (rclc_support_init(&this->support, 0, NULL, &this->allocator) != RCL_RET_OK) {
            printk("Can't init support!\r\n");
            return false;
        }
        support_init = true;
    }
        
    if (!defaults_init){
        if (rclc_node_init_default(&this->node, this->node_name, "", &this->support) != RCL_RET_OK){
            printk("Can't init defaults!\r\n");
            return false;
        }
        defaults_init = true;
    }

    if (!executor_init){
        if (rclc_executor_init(&this->executor, &this->support.context, 32, &this->allocator) != RCL_RET_OK){
            printk("Can't init executor!\r\n");
            return false;
        }
        executor_init = true;
    }

    return true;
}

bool MicrorosExecutor::execute() {
    while (1) {
        rclc_executor_spin_some(&this->executor, 100);
        k_sleep(K_MSEC(10));
    }
}

rcl_publisher_t* MicrorosExecutor::add_publisher(const rosidl_message_type_support_t *type_support, const char *topic_name) {
    // Allocate new list node
    struct publisher_node *pub_node = (struct publisher_node *)malloc(sizeof(struct publisher_node));
    if (!pub_node) return nullptr;

    if (rclc_publisher_init_default(&pub_node->publisher, &this->node, type_support, topic_name) != RCL_RET_OK) {
        free(pub_node);
        return nullptr;
    }

    // Append to list
    sys_slist_append(&this->publishers_head, &pub_node->node);

    printk("registered publisher %s\n", topic_name);
    return &pub_node->publisher;
}

rcl_subscription_t* MicrorosExecutor::add_subscription(const rosidl_message_type_support_t *type_support, const char *topic_name, void* msg, rclc_callback_t callback) {
    // Allocate new list node
    struct subscription_node *sub_node = (struct subscription_node *)malloc(sizeof(struct subscription_node));
    if (!sub_node) return nullptr;

    if (rclc_subscription_init_default(&sub_node->subscription, &this->node, type_support, topic_name) != RCL_RET_OK) {
        free(sub_node);
        return nullptr;
    }

    // Append to list
    sys_slist_append(&this->subscribers_head, &sub_node->node);

    if (rclc_executor_add_subscription(&this->executor, &sub_node->subscription, msg, callback, ON_NEW_DATA) != RCL_RET_OK) {
        free(sub_node);
        return nullptr;
    }

    printk("registered subscriber %s\n", topic_name);
    return &sub_node->subscription;
}